Fixed DNA issue, some optional SSE stuff in (experimental, only 2 functions => not...
[blender.git] / source / blender / blenkernel / intern / implicit.c
1 /*  implicit.c      
2
3 *
4 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version. The Blender
10 * Foundation also sells licenses for use in proprietary software under
11 * the Blender License.  See http://www.blender.org/BL/ for information
12 * about this.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software Foundation,
21 * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22 *
23 * The Original Code is Copyright (C) Blender Foundation
24 * All rights reserved.
25 *
26 * The Original Code is: all of this file.
27 *
28 * Contributor(s): none yet.
29 *
30 * ***** END GPL/BL DUAL LICENSE BLOCK *****
31 */
32 #include <math.h>
33 #include <stdlib.h>
34 #include <string.h>
35 #include <stdio.h>
36 #include "MEM_guardedalloc.h"
37 /* types */
38 #include "DNA_curve_types.h"
39 #include "DNA_object_types.h"
40 #include "DNA_object_force.h"   
41 #include "DNA_cloth_types.h"    
42 #include "DNA_key_types.h"
43 #include "DNA_mesh_types.h"
44 #include "DNA_modifier_types.h"
45 #include "DNA_meshdata_types.h"
46 #include "DNA_lattice_types.h"
47 #include "DNA_scene_types.h"
48 #include "DNA_modifier_types.h"
49 #include "BLI_arithb.h"
50 #include "BLI_blenlib.h"
51 #include "BLI_edgehash.h"
52 #include "BLI_threads.h"
53 #include "BKE_collisions.h"
54 #include "BKE_curve.h"
55 #include "BKE_displist.h"
56 #include "BKE_effect.h"
57 #include "BKE_global.h"
58 #include "BKE_key.h"
59 #include "BKE_object.h"
60 #include "BKE_cloth.h"
61 #include "BKE_modifier.h"
62 #include "BKE_utildefines.h"
63 #include "BKE_global.h"
64 #include  "BIF_editdeform.h"
65
66 #include "Bullet-C-Api.h"
67
68 #include <emmintrin.h>
69 #include <xmmintrin.h>
70 #include <pmmintrin.h>
71
72 #ifdef _WIN32
73 #include <windows.h>
74 static LARGE_INTEGER _itstart, _itend;
75 static LARGE_INTEGER ifreq;
76 void itstart(void)
77 {
78         static int first = 1;
79         if(first) {
80                 QueryPerformanceFrequency(&ifreq);
81                 first = 0;
82         }
83         QueryPerformanceCounter(&_itstart);
84 }
85 void itend(void)
86 {
87         QueryPerformanceCounter(&_itend);
88 }
89 double itval()
90 {
91         return ((double)_itend.QuadPart -
92                 (double)_itstart.QuadPart)/((double)ifreq.QuadPart);
93 }
94 #else
95 #include <sys/time.h>
96
97 static struct timeval _itstart, _itend;
98 static struct timezone itz;
99 void itstart(void)
100 {
101         gettimeofday(&_itstart, &itz);
102 }
103 void itend(void)
104 {
105         gettimeofday(&_itend,&itz);
106 }
107 double itval()
108 {
109         double t1, t2;
110         t1 =  (double)_itstart.tv_sec + (double)_itstart.tv_usec/(1000*1000);
111         t2 =  (double)_itend.tv_sec + (double)_itend.tv_usec/(1000*1000);
112         return t2-t1;
113 }
114 #endif
115
116 struct Cloth;
117
118 //////////////////////////////////////////
119 /* fast vector / matrix library, enhancements are welcome :) -dg */
120 /////////////////////////////////////////
121
122 /* DEFINITIONS */
123 #ifdef __GNUC__
124 typedef float lfVector[4] __attribute__ ((aligned (16)));
125 #else
126 typedef __declspec(align(16)) lfVector[4];
127 #endif
128
129 #ifdef __GNUC__
130 typedef struct fmatrix3x3 {
131         float m[3][4] __attribute__ ((aligned (16))); /* 3x3 matrix */
132         unsigned int c,r; /* column and row number */
133         int pinned; /* is this vertex allowed to move? */
134         float n1,n2,n3; /* three normal vectors for collision constrains */
135         unsigned int vcount; /* vertex count */
136         unsigned int scount; /* spring count */ 
137 } fmatrix3x3;
138 #else
139 typedef struct fmatrix3x3 {
140         __declspec(align(16))
141         float m[3][4]; /* 3x3 matrix */
142         unsigned int c,r; /* column and row number */
143         int pinned; /* is this vertex allowed to move? */
144         float n1,n2,n3; /* three normal vectors for collision constrains */
145         unsigned int vcount; /* vertex count */
146         unsigned int scount; /* spring count */ 
147 } fmatrix3x3;
148 #endif
149
150
151 ///////////////////////////
152 // float[3] vector
153 ///////////////////////////
154 /* simple vector code */
155
156 /* STATUS: verified */
157 DO_INLINE void mul_fvector_S(float to[3], float from[3], float scalar)
158 {
159         to[0] = from[0] * scalar;
160         to[1] = from[1] * scalar;
161         to[2] = from[2] * scalar;
162 }
163 /* simple cross product */
164 /* STATUS: verified */
165 DO_INLINE void cross_fvector(float to[3], float vectorA[3], float vectorB[3])
166 {
167         float temp[3];
168         
169         temp[0] = vectorA[1] * vectorB[2] - vectorA[2] * vectorB[1];
170         temp[1] = vectorA[2] * vectorB[0] - vectorA[0] * vectorB[2];
171         temp[2] = vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0];
172         
173         VECCOPY(to, temp);
174 }
175
176 /* simple v^T * v product ("outer product") */
177 /* STATUS: HAS TO BE verified (*should* work) */
178 DO_INLINE void mul_fvectorT_fvector(float to[3][4], float vectorA[3], float vectorB[3])
179 {
180         mul_fvector_S(to[0], vectorB, vectorA[0]);
181         mul_fvector_S(to[1], vectorB, vectorA[1]);
182         mul_fvector_S(to[2], vectorB, vectorA[2]);
183 }
184 /* simple v^T * v product with scalar ("outer product") */
185 /* STATUS: HAS TO BE verified (*should* work) */
186 DO_INLINE void mul_fvectorT_fvectorS(float to[3][4], float vectorA[3], float vectorB[3], float aS)
187 {
188         mul_fvector_S(to[0], vectorB, vectorA[0]* aS);
189         mul_fvector_S(to[1], vectorB, vectorA[1]* aS);
190         mul_fvector_S(to[2], vectorB, vectorA[2]* aS);
191 }
192
193 /* printf vector[3] on console: for debug output */
194 void print_fvector(float m3[3])
195 {
196         printf("%f\n%f\n%f\n\n",m3[0],m3[1],m3[2]);
197 }
198
199 ///////////////////////////
200 // long float vector float (*)[3]
201 ///////////////////////////
202 /* print long vector on console: for debug output */
203 DO_INLINE void print_lfvector(lfVector *fLongVector, unsigned int verts)
204 {
205         unsigned int i = 0;
206         for(i = 0; i < verts; i++)
207         {
208                 print_fvector(fLongVector[i]);
209         }
210 }
211 /* create long vector */
212 DO_INLINE lfVector *create_lfvector(unsigned int verts)
213 {
214         // TODO: check if memory allocation was successfull */
215         return  (lfVector *)MEM_callocN (verts * sizeof(lfVector), "cloth_implicit_alloc_vector");
216         // return (lfVector *)cloth_aligned_malloc(&MEMORY_BASE, verts * sizeof(lfVector));
217 }
218 /* delete long vector */
219 DO_INLINE void del_lfvector(lfVector *fLongVector)
220 {
221         if (fLongVector != NULL)
222         {
223                 MEM_freeN (fLongVector);
224                 // cloth_aligned_free(&MEMORY_BASE, fLongVector);
225         }
226 }
227 /* copy long vector */
228 DO_INLINE void cp_lfvector(lfVector *to, lfVector *from, unsigned int verts)
229 {
230         memcpy(to, from, verts * sizeof(lfVector));
231 }
232 /* init long vector with float[3] */
233 DO_INLINE void init_lfvector(lfVector *fLongVector, float vector[3], unsigned int verts)
234 {
235         unsigned int i = 0;
236         for(i = 0; i < verts; i++)
237         {
238                 VECCOPY(fLongVector[i], vector);
239         }
240 }
241 /* zero long vector with float[3] */
242 DO_INLINE void zero_lfvector(lfVector *to, unsigned int verts)
243 {
244         memset(to, 0.0f, verts * sizeof(lfVector));
245 }
246 /* multiply long vector with scalar*/
247 DO_INLINE void mul_lfvectorS(lfVector *to, lfVector *fLongVector, float scalar, unsigned int verts)
248 {
249         unsigned int i = 0;
250
251         for(i = 0; i < verts; i++)
252         {
253                 mul_fvector_S(to[i], fLongVector[i], scalar);
254         }
255 }
256 /* multiply long vector with scalar*/
257 /* A -= B * float */
258 DO_INLINE void submul_lfvectorS(lfVector *to, lfVector *fLongVector, float scalar, unsigned int verts)
259 {
260         unsigned int i = 0;
261         for(i = 0; i < verts; i++)
262         {
263                 VECSUBMUL(to[i], fLongVector[i], scalar);
264         }
265 }
266 /* dot product for big vector */
267 DO_INLINE float dot_lfvector(lfVector *fLongVectorA, lfVector *fLongVectorB, unsigned int verts)
268 {
269         unsigned int i = 0;
270         float temp = 0.0;
271 // schedule(guided, 2)
272 #pragma omp parallel for reduction(+: temp) schedule(static)
273         for(i = 0; i < verts; i++)
274         {
275                 temp += INPR(fLongVectorA[i], fLongVectorB[i]);
276         }
277         return temp;
278 }
279 /* A = B + C  --> for big vector */
280 DO_INLINE void add_lfvector_lfvector(lfVector *to, lfVector *fLongVectorA, lfVector *fLongVectorB, unsigned int verts)
281 {
282         unsigned int i = 0;
283
284         for(i = 0; i < verts; i++)
285         {
286                 VECADD(to[i], fLongVectorA[i], fLongVectorB[i]);
287         }
288
289 }
290 /* A = B + C * float --> for big vector */
291 DO_INLINE void add_lfvector_lfvectorS(lfVector *to, lfVector *fLongVectorA, lfVector *fLongVectorB, float bS, unsigned int verts)
292 {
293         unsigned int i = 0;
294
295         for(i = 0; i < verts; i++)
296         {
297                 VECADDS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
298
299         }
300 }
301 /* A = B * float + C * float --> for big vector */
302 DO_INLINE void add_lfvectorS_lfvectorS(lfVector *to, lfVector *fLongVectorA, float aS, lfVector *fLongVectorB, float bS, unsigned int verts)
303 {
304         unsigned int i = 0;
305
306         for(i = 0; i < verts; i++)
307         {
308                 VECADDSS(to[i], fLongVectorA[i], aS, fLongVectorB[i], bS);
309         }
310 }
311 /* A = B - C * float --> for big vector */
312 DO_INLINE void sub_lfvector_lfvectorS(lfVector *to, lfVector *fLongVectorA, lfVector *fLongVectorB, float bS, unsigned int verts)
313 {
314         unsigned int i = 0;
315         for(i = 0; i < verts; i++)
316         {
317                 VECSUBS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
318         }
319
320 }
321 /* A = B - C --> for big vector */
322 DO_INLINE void sub_lfvector_lfvector(lfVector *to, lfVector *fLongVectorA, lfVector *fLongVectorB, unsigned int verts)
323 {
324         unsigned int i = 0;
325
326         for(i = 0; i < verts; i++)
327         {
328                 VECSUB(to[i], fLongVectorA[i], fLongVectorB[i]);
329         }
330
331 }
332 ///////////////////////////
333 // 3x3 matrix
334 ///////////////////////////
335 /* printf 3x3 matrix on console: for debug output */
336 void print_fmatrix(float m3[3][4])
337 {
338         printf("%f\t%f\t%f\n",m3[0][0],m3[0][1],m3[0][2]);
339         printf("%f\t%f\t%f\n",m3[1][0],m3[1][1],m3[1][2]);
340         printf("%f\t%f\t%f\n\n",m3[2][0],m3[2][1],m3[2][2]);
341 }
342 /* copy 3x3 matrix */
343 DO_INLINE void cp_fmatrix(float to[3][4], float from[3][4])
344 {
345         memcpy(to, from, sizeof (float) * 12);
346         /*
347         VECCOPY(to[0], from[0]);
348         VECCOPY(to[1], from[1]);
349         VECCOPY(to[2], from[2]);
350         */
351 }
352 /* calculate determinant of 3x3 matrix */
353 DO_INLINE float det_fmatrix(float m[3][4])
354 {
355         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] 
356         -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];
357 }
358 DO_INLINE void inverse_fmatrix(float to[3][4], float from[3][4])
359 {
360         unsigned int i, j;
361         float d;
362
363         if((d=det_fmatrix(from))==0)
364         {
365                 printf("can't build inverse");
366                 exit(0);
367         }
368         for(i=0;i<3;i++) 
369         {
370                 for(j=0;j<3;j++) 
371                 {
372                         int i1=(i+1)%3;
373                         int i2=(i+2)%3;
374                         int j1=(j+1)%3;
375                         int j2=(j+2)%3;
376                         // reverse indexs i&j to take transpose
377                         to[j][i] = (from[i1][j1]*from[i2][j2]-from[i1][j2]*from[i2][j1])/d;
378                         /*
379                         if(i==j)
380                         to[i][j] = 1.0f / from[i][j];
381                         else
382                         to[i][j] = 0;
383                         */
384                 }
385         }
386
387 }
388
389 /* 3x3 matrix multiplied by a scalar */
390 /* STATUS: verified */
391 DO_INLINE void mul_fmatrix_S(float matrix[3][4], float scalar)
392 {
393         mul_fvector_S(matrix[0], matrix[0],scalar);
394         mul_fvector_S(matrix[1], matrix[1],scalar);
395         mul_fvector_S(matrix[2], matrix[2],scalar);
396 }
397
398 /* a vector multiplied by a 3x3 matrix */
399 /* STATUS: verified */
400 DO_INLINE void mul_fvector_fmatrix(float *to, float *from, float matrix[3][4])
401 {
402         float temp[3];
403         
404         VECCOPY(temp, from);
405         
406         to[0] = matrix[0][0]*temp[0] + matrix[1][0]*temp[1] + matrix[2][0]*temp[2];
407         to[1] = matrix[0][1]*temp[0] + matrix[1][1]*temp[1] + matrix[2][1]*temp[2];
408         to[2] = matrix[0][2]*temp[0] + matrix[1][2]*temp[1] + matrix[2][2]*temp[2];
409 }
410
411 /* 3x3 matrix multiplied by a vector */
412 /* STATUS: verified */
413 #ifdef SSE3
414 DO_INLINE void mul_fmatrix_fvector(float *to, float matrix[3][4], float *from) {
415         __m128 v1, v2, v3, v4;
416         
417         v1 = _mm_load_ps(&matrix[0]);
418         v2 = _mm_load_ps(&matrix[1]);
419         v3 = _mm_load_ps(&matrix[2]);
420         v4 = _mm_load_ps(from);
421
422         // stuff
423         v1 = _mm_mul_ps(v1, v4);
424         v2 = _mm_mul_ps(v2, v4);
425         v3 = _mm_mul_ps(v3, v4);
426         v1 = _mm_hadd_ps(v1, v2);
427         v3 = _mm_hadd_ps(v3, _mm_setzero_ps());
428         v1 = _mm_hadd_ps(v1, v3);
429         
430         _mm_store_ps(to, v4);
431 }
432 #else
433 DO_INLINE void mul_fmatrix_fvector(float *to, float matrix[3][4], float *from)
434 {
435         to[0] = INPR(matrix[0],from);
436         to[1] = INPR(matrix[1],from);
437         to[2] = INPR(matrix[2],from);
438 }
439 #endif
440                 
441 /* 3x3 matrix multiplied by a 3x3 matrix */
442 /* STATUS: verified */
443 DO_INLINE void mul_fmatrix_fmatrix(float to[3][4], float matrixA[3][4], float matrixB[3][4])
444 {
445         mul_fvector_fmatrix(to[0], matrixA[0],matrixB);
446         mul_fvector_fmatrix(to[1], matrixA[1],matrixB);
447         mul_fvector_fmatrix(to[2], matrixA[2],matrixB);
448 }
449 /* 3x3 matrix addition with 3x3 matrix */
450 DO_INLINE void add_fmatrix_fmatrix(float to[3][4], float matrixA[3][4], float matrixB[3][4])
451 {
452         VECADD(to[0], matrixA[0], matrixB[0]);
453         VECADD(to[1], matrixA[1], matrixB[1]);
454         VECADD(to[2], matrixA[2], matrixB[2]);
455 }
456 /* 3x3 matrix add-addition with 3x3 matrix */
457 DO_INLINE void addadd_fmatrix_fmatrix(float to[3][4], float matrixA[3][4], float matrixB[3][4])
458 {
459         VECADDADD(to[0], matrixA[0], matrixB[0]);
460         VECADDADD(to[1], matrixA[1], matrixB[1]);
461         VECADDADD(to[2], matrixA[2], matrixB[2]);
462 }
463 /* 3x3 matrix sub-addition with 3x3 matrix */
464 DO_INLINE void addsub_fmatrixS_fmatrixS(float to[3][4], float matrixA[3][4], float aS, float matrixB[3][4], float bS)
465 {
466         VECADDSUBSS(to[0], matrixA[0], aS, matrixB[0], bS);
467         VECADDSUBSS(to[1], matrixA[1], aS, matrixB[1], bS);
468         VECADDSUBSS(to[2], matrixA[2], aS, matrixB[2], bS);
469 }
470 /* A -= B + C (3x3 matrix sub-addition with 3x3 matrix) */
471 DO_INLINE void subadd_fmatrix_fmatrix(float to[3][4], float matrixA[3][4], float matrixB[3][4])
472 {
473         VECSUBADD(to[0], matrixA[0], matrixB[0]);
474         VECSUBADD(to[1], matrixA[1], matrixB[1]);
475         VECSUBADD(to[2], matrixA[2], matrixB[2]);
476 }
477 /* A -= B*x + C*y (3x3 matrix sub-addition with 3x3 matrix) */
478 DO_INLINE void subadd_fmatrixS_fmatrixS(float to[3][4], float matrixA[3][4], float aS, float matrixB[3][4], float bS)
479 {
480         VECSUBADDSS(to[0], matrixA[0], aS, matrixB[0], bS);
481         VECSUBADDSS(to[1], matrixA[1], aS, matrixB[1], bS);
482         VECSUBADDSS(to[2], matrixA[2], aS, matrixB[2], bS);
483 }
484 /* A = B - C (3x3 matrix subtraction with 3x3 matrix) */
485 DO_INLINE void sub_fmatrix_fmatrix(float to[3][4], float matrixA[3][4], float matrixB[3][4])
486 {
487         VECSUB(to[0], matrixA[0], matrixB[0]);
488         VECSUB(to[1], matrixA[1], matrixB[1]);
489         VECSUB(to[2], matrixA[2], matrixB[2]);
490 }
491 /* A += B - C (3x3 matrix add-subtraction with 3x3 matrix) */
492 DO_INLINE void addsub_fmatrix_fmatrix(float to[3][4], float matrixA[3][4], float matrixB[3][4])
493 {
494         VECADDSUB(to[0], matrixA[0], matrixB[0]);
495         VECADDSUB(to[1], matrixA[1], matrixB[1]);
496         VECADDSUB(to[2], matrixA[2], matrixB[2]);
497 }
498 /////////////////////////////////////////////////////////////////
499 // special functions
500 /////////////////////////////////////////////////////////////////
501 /* a vector multiplied and added to/by a 3x3 matrix */
502 /*
503 DO_INLINE void muladd_fvector_fmatrix(float to[3], float from[3], float matrix[3][3])
504 {
505         to[0] += matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
506         to[1] += matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
507         to[2] += matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
508 }
509 */
510 /* 3x3 matrix multiplied and added  to/by a 3x3 matrix  and added to another 3x3 matrix */
511 /*
512 DO_INLINE void muladd_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
513 {
514         muladd_fvector_fmatrix(to[0], matrixA[0],matrixB);
515         muladd_fvector_fmatrix(to[1], matrixA[1],matrixB);
516         muladd_fvector_fmatrix(to[2], matrixA[2],matrixB);
517 }
518 */
519 /* a vector multiplied and sub'd to/by a 3x3 matrix */
520 /*
521 DO_INLINE void mulsub_fvector_fmatrix(float to[3], float from[3], float matrix[3][3])
522 {
523         to[0] -= matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
524         to[1] -= matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
525         to[2] -= matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
526 }
527 */
528 /* 3x3 matrix multiplied and sub'd  to/by a 3x3 matrix  and added to another 3x3 matrix */
529 /*
530 DO_INLINE void mulsub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
531 {
532         mulsub_fvector_fmatrix(to[0], matrixA[0],matrixB);
533         mulsub_fvector_fmatrix(to[1], matrixA[1],matrixB);
534         mulsub_fvector_fmatrix(to[2], matrixA[2],matrixB);
535 }
536 */
537 /* 3x3 matrix multiplied+added by a vector */
538 /* STATUS: verified */
539
540 #ifdef SSE3
541 DO_INLINE void muladd_fmatrix_fvector(float to[3], float matrix[3][4], float from[3]) {
542         __m128 v1, v2, v3, v4;
543         
544         v1 = _mm_load_ps(&matrix[0]);
545         v2 = _mm_load_ps(&matrix[1]);
546         v3 = _mm_load_ps(&matrix[2]);
547         v4 = _mm_load_ps(from);
548
549         // stuff
550         v1 = _mm_mul_ps(v1, v4);
551         v2 = _mm_mul_ps(v2, v4);
552         v3 = _mm_mul_ps(v3, v4);
553         v1 = _mm_hadd_ps(v1, v2);
554         v3 = _mm_hadd_ps(v3, _mm_setzero_ps());
555         v1 = _mm_hadd_ps(v1, v3);
556         
557         v4 = _mm_load_ps(to);
558         v4 = _mm_add_ps(v4,v1);
559
560         _mm_store_ps(to, v4);
561 }
562 #else
563 DO_INLINE void muladd_fmatrix_fvector(float to[3], float matrix[3][4], float from[3])
564 {
565         float temp[3] = { 0,0,0 };
566         
567         temp[0] = INPR(matrix[0],from);
568         temp[1] = INPR(matrix[1],from);
569         temp[2] = INPR(matrix[2],from); 
570         
571         VECADD(to, to, temp);
572 }
573 #endif
574
575 /* 3x3 matrix multiplied+sub'ed by a vector */
576 /*
577 DO_INLINE void mulsub_fmatrix_fvector(float to[3], float matrix[3][3], float from[3])
578 {
579         to[0] -= INPR(matrix[0],from);
580         to[1] -= INPR(matrix[1],from);
581         to[2] -= INPR(matrix[2],from);
582 }
583 */
584 /////////////////////////////////////////////////////////////////
585
586 ///////////////////////////
587 // SPARSE SYMMETRIC big matrix with 3x3 matrix entries
588 ///////////////////////////
589 /* printf a big matrix on console: for debug output */
590 void print_bfmatrix(fmatrix3x3 *m3)
591 {
592         unsigned int i = 0;
593
594         for(i = 0; i < m3[0].vcount + m3[0].scount; i++)
595         {
596                 print_fmatrix(m3[i].m);
597         }
598 }
599 /* create big matrix */
600 DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs)
601 {
602         // TODO: check if memory allocation was successfull */
603         fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN (sizeof (fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix");
604         temp[0].vcount = verts;
605         temp[0].scount = springs;
606         return temp;
607 }
608 /* delete big matrix */
609 DO_INLINE void del_bfmatrix(fmatrix3x3 *matrix)
610 {
611         if (matrix != NULL)
612         {
613                 MEM_freeN (matrix);
614         }
615 }
616 /* copy big matrix */
617 DO_INLINE void cp_bfmatrix(fmatrix3x3 *to, fmatrix3x3 *from)
618 {       
619         // TODO bounds checking 
620         memcpy(to, from, sizeof(fmatrix3x3) * (from[0].vcount+from[0].scount) );
621 }
622 /* init the diagonal of big matrix */
623 // slow in parallel
624 DO_INLINE void initdiag_bfmatrix(fmatrix3x3 *matrix, float m3[3][4])
625 {
626         unsigned int i,j;
627         float tmatrix[3][4] = {{0,0,0,0},{0,0,0,0},{0,0,0,0}};
628
629         for(i = 0; i < matrix[0].vcount; i++)
630         {               
631                 cp_fmatrix(matrix[i].m, m3); 
632         }
633         for(j = matrix[0].vcount; j < matrix[0].vcount+matrix[0].scount; j++)
634         {
635                 cp_fmatrix(matrix[j].m, tmatrix); 
636         }
637 }
638 /* init big matrix */
639 DO_INLINE void init_bfmatrix(fmatrix3x3 *matrix, float m3[3][4])
640 {
641         unsigned int i;
642
643         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
644         {
645                 cp_fmatrix(matrix[i].m, m3); 
646         }
647 }
648 /* multiply big matrix with scalar*/
649 DO_INLINE void mul_bfmatrix_S(fmatrix3x3 *matrix, float scalar)
650 {
651         unsigned int i = 0;
652         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
653         {
654                 mul_fmatrix_S(matrix[i].m, scalar);
655         }
656 }
657 /* SPARSE SYMMETRIC multiply big matrix with long vector*/
658 /* STATUS: verified */
659 void mul_bfmatrix_lfvector( lfVector *to, fmatrix3x3 *from, lfVector *fLongVector)
660 {
661         unsigned int i = 0;
662         float *tflongvector;
663         float temp[4]={0,0,0,0};
664         
665         zero_lfvector(to, from[0].vcount);
666         
667         /* process diagonal elements */ 
668         for(i = 0; i < from[0].vcount; i++)
669         {
670                 mul_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);
671         }
672
673         /* process off-diagonal entries (every off-diagonal entry needs to be symmetric) */
674         // TODO: pragma below is wrong, correct it!
675 // #pragma omp parallel for shared(to,from, fLongVector) private(i) 
676         
677         for(i = from[0].vcount; i < from[0].vcount+from[0].scount; i++)
678         {
679                 muladd_fmatrix_fvector(to[from[i].c], from[i].m, fLongVector[from[i].r]);
680                 muladd_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);
681         }
682 }
683
684 /* SPARSE SYMMETRIC add big matrix with big matrix: A = B + C*/
685 DO_INLINE void add_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
686 {
687         unsigned int i = 0;
688
689         /* process diagonal elements */
690         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
691         {
692                 add_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
693         }
694
695 }
696 /* SPARSE SYMMETRIC add big matrix with big matrix: A += B + C */
697 DO_INLINE void addadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
698 {
699         unsigned int i = 0;
700
701         /* process diagonal elements */
702         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
703         {
704                 addadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
705         }
706
707 }
708 /* SPARSE SYMMETRIC subadd big matrix with big matrix: A -= B + C */
709 DO_INLINE void subadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
710 {
711         unsigned int i = 0;
712
713         /* process diagonal elements */
714         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
715         {
716                 subadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
717         }
718
719 }
720 /*  A = B - C (SPARSE SYMMETRIC sub big matrix with big matrix) */
721 DO_INLINE void sub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
722 {
723         unsigned int i = 0;
724
725         /* process diagonal elements */
726         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
727         {
728                 sub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
729         }
730
731 }
732 /* SPARSE SYMMETRIC sub big matrix with big matrix S (special constraint matrix with limited entries) */
733 DO_INLINE void sub_bfmatrix_Smatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
734 {
735         unsigned int i = 0;
736
737         /* process diagonal elements */
738         for(i = 0; i < matrix[0].vcount; i++)
739         {
740                 sub_fmatrix_fmatrix(to[matrix[i].c].m, from[matrix[i].c].m, matrix[i].m);       
741         }
742
743 }
744 /* A += B - C (SPARSE SYMMETRIC addsub big matrix with big matrix) */
745 DO_INLINE void addsub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
746 {
747         unsigned int i = 0;
748
749         /* process diagonal elements */
750         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
751         {
752                 addsub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
753         }
754
755 }
756 /* SPARSE SYMMETRIC sub big matrix with big matrix*/
757 /* A -= B * float + C * float --> for big matrix */
758 /* VERIFIED */
759 DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, float aS,  fmatrix3x3 *matrix, float bS)
760 {
761         unsigned int i = 0;
762
763         /* process diagonal elements */
764         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
765         {
766                 subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS);      
767         }
768
769 }
770
771 ///////////////////////////////////////////////////////////////////
772 // simulator start
773 ///////////////////////////////////////////////////////////////////
774 static float I[3][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0}};
775 static float ZERO[3][4] = {{0,0,0,0},{0,0,0,0},{0,0,0,0}};
776 typedef struct Implicit_Data 
777 {
778         lfVector *X, *V, *Xnew, *Vnew, *F, *B, *dV, *z;
779         fmatrix3x3 *A, *dFdV, *dFdX, *S, *P, *Pinv, *bigI; 
780 } Implicit_Data;
781
782 int implicit_init (Object *ob, ClothModifierData *clmd)
783 {
784         unsigned int i = 0;
785         unsigned int pinned = 0;
786         Cloth *cloth = NULL;
787         ClothVertex *verts = NULL;
788         ClothSpring *spring = NULL;
789         Implicit_Data *id = NULL;
790         LinkNode *search = NULL;
791
792         // init memory guard
793         // MEMORY_BASE.first = MEMORY_BASE.last = NULL;
794
795         cloth = (Cloth *)clmd->clothObject;
796         verts = cloth->verts;
797
798         // create implicit base
799         id = (Implicit_Data *)MEM_callocN (sizeof(Implicit_Data), "implicit vecmat");
800         cloth->implicit = id;
801
802         /* process diagonal elements */         
803         id->A = create_bfmatrix(cloth->numverts, cloth->numsprings);
804         id->dFdV = create_bfmatrix(cloth->numverts, cloth->numsprings);
805         id->dFdX = create_bfmatrix(cloth->numverts, cloth->numsprings);
806         id->S = create_bfmatrix(cloth->numverts, 0);
807         id->Pinv = create_bfmatrix(cloth->numverts, cloth->numsprings);
808         id->P = create_bfmatrix(cloth->numverts, cloth->numsprings);
809         id->bigI = create_bfmatrix(cloth->numverts, cloth->numsprings); // TODO 0 springs
810         id->X = create_lfvector(cloth->numverts);
811         id->Xnew = create_lfvector(cloth->numverts);
812         id->V = create_lfvector(cloth->numverts);
813         id->Vnew = create_lfvector(cloth->numverts);
814         id->F = create_lfvector(cloth->numverts);
815         id->B = create_lfvector(cloth->numverts);
816         id->dV = create_lfvector(cloth->numverts);
817         zero_lfvector(id->dV, cloth->numverts);
818         id->z = create_lfvector(cloth->numverts);
819         
820         for(i=0;i<cloth->numverts;i++) 
821         {
822                 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 = i;
823
824                 if(verts [i].goal >= SOFTGOALSNAP)
825                 {
826                         id->S[pinned].pinned = 1;
827                         id->S[pinned].c = id->S[pinned].r = i;
828                         pinned++;
829                 }
830         }
831
832         // S is special and needs specific vcount and scount
833         id->S[0].vcount = pinned; id->S[0].scount = 0;
834
835         // init springs */
836         search = cloth->springs;
837         for(i=0;i<cloth->numsprings;i++) 
838         {
839                 spring = search->link;
840                 
841                 // dFdV_start[i].r = big_I[i].r = big_zero[i].r = 
842                 id->A[i+cloth->numverts].r = id->dFdV[i+cloth->numverts].r = id->dFdX[i+cloth->numverts].r = 
843                                 id->P[i+cloth->numverts].r = id->Pinv[i+cloth->numverts].r = id->bigI[i+cloth->numverts].r = spring->ij;
844
845                 // dFdV_start[i].c = big_I[i].c = big_zero[i].c = 
846                 id->A[i+cloth->numverts].c = id->dFdV[i+cloth->numverts].c = id->dFdX[i+cloth->numverts].c = 
847                         id->P[i+cloth->numverts].c = id->Pinv[i+cloth->numverts].c = id->bigI[i+cloth->numverts].c = spring->kl;
848
849                 spring->matrix_index = i + cloth->numverts;
850                 
851                 search = search->next;
852         }
853
854         for(i = 0; i < cloth->numverts; i++)
855         {               
856                 VECCOPY(id->X[i], cloth->x[i]);
857         }
858
859         return 1;
860 }
861 int     implicit_free (ClothModifierData *clmd)
862 {
863         Implicit_Data *id;
864         Cloth *cloth;
865         cloth = (Cloth *)clmd->clothObject;
866
867         if(cloth)
868         {
869                 id = cloth->implicit;
870
871                 if(id)
872                 {
873                         del_bfmatrix(id->A);
874                         del_bfmatrix(id->dFdV);
875                         del_bfmatrix(id->dFdX);
876                         del_bfmatrix(id->S);
877                         del_bfmatrix(id->P);
878                         del_bfmatrix(id->Pinv);
879                         del_bfmatrix(id->bigI);
880
881                         del_lfvector(id->X);
882                         del_lfvector(id->Xnew);
883                         del_lfvector(id->V);
884                         del_lfvector(id->Vnew);
885                         del_lfvector(id->F);
886                         del_lfvector(id->B);
887                         del_lfvector(id->dV);
888                         del_lfvector(id->z);
889
890                         MEM_freeN(id);
891                 }
892         }
893
894         return 1;
895 }
896
897 void cloth_bending_mode(ClothModifierData *clmd, int enabled)
898 {
899         Cloth *cloth = clmd->clothObject;
900         Implicit_Data *id;
901         
902         if(cloth)
903         {
904                 id = cloth->implicit;
905                 
906                 if(id)
907                 {
908                         if(enabled)
909                         {
910                                 cloth->numsprings = cloth->numspringssave;
911                         }
912                         else
913                         {
914                                 cloth->numsprings = cloth->numothersprings;
915                         }
916                         
917                         id->A[0].scount = id->dFdV[0].scount = id->dFdX[0].scount = id->P[0].scount = id->Pinv[0].scount = id->bigI[0].scount = cloth->numsprings;
918                 }       
919         }
920 }
921
922 DO_INLINE float fb(float length, float L)
923 {
924         float x = length/L;
925         return (-11.541f*pow(x,4)+34.193f*pow(x,3)-39.083f*pow(x,2)+23.116f*x-9.713f);
926 }
927
928 DO_INLINE float fbderiv(float length, float L)
929 {
930         float x = length/L;
931
932         return (-46.164f*pow(x,3)+102.579f*pow(x,2)-78.166f*x+23.116f);
933 }
934
935 DO_INLINE float fbstar(float length, float L, float kb, float cb)
936 {
937         float tempfb = kb * fb(length, L);
938
939         float fbstar = cb * (length - L);
940
941         if(tempfb < fbstar)
942                 return fbstar;
943         else
944                 return tempfb;          
945 }
946
947 DO_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
948 {
949         float tempfb = kb * fb(length, L);
950         float fbstar = cb * (length - L);
951
952         if(tempfb < fbstar)
953         {               
954                 return cb;
955         }
956         else
957         {
958                 return kb * fbderiv(length, L); 
959         }       
960 }
961
962 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
963 {
964         unsigned int i=0;
965
966         for(i=0;i<S[0].vcount;i++)
967         {
968                 mul_fvector_fmatrix(V[S[i].r], V[S[i].r], S[i].m);
969         }
970 }
971
972 int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S)
973 {
974         // Solves for unknown X in equation AX=B
975         unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
976         float conjgrad_epsilon=0.0001f, conjgrad_lasterror=0;
977         lfVector *q, *d, *tmp, *r; 
978         float s, starget, a, s_prev;
979         unsigned int numverts = lA[0].vcount;
980         q = create_lfvector(numverts);
981         d = create_lfvector(numverts);
982         tmp = create_lfvector(numverts);
983         r = create_lfvector(numverts);
984         
985         // zero_lfvector(ldV, numverts);
986         filter(ldV, S);
987         add_lfvector_lfvector(ldV, ldV, z, numverts);
988
989         // r = B - Mul(tmp,A,X);    // just use B if X known to be zero
990         mul_bfmatrix_lfvector(r, lA, ldV);
991         sub_lfvector_lfvector(r, lB, r, numverts);
992         filter(r, S);
993
994         cp_lfvector(d, r, numverts);
995
996         s = dot_lfvector(r, r, numverts);
997         starget = s * conjgrad_epsilon;
998         
999         // itstart();
1000         while((s>starget && conjgrad_loopcount < conjgrad_looplimit))
1001         {       
1002                 // Mul(q,A,d); // q = A*d;
1003                 mul_bfmatrix_lfvector(q, lA, d);
1004
1005                 filter(q,S);
1006
1007                 a = s/dot_lfvector(d, q, numverts);
1008
1009                 // X = X + d*a;
1010                 add_lfvector_lfvectorS(ldV, ldV, d, a, numverts);
1011
1012                 // r = r - q*a;
1013                 sub_lfvector_lfvectorS(r, r, q, a, numverts);
1014
1015                 s_prev = s;
1016                 s = dot_lfvector(r, r, numverts);
1017
1018                 //d = r+d*(s/s_prev);
1019                 add_lfvector_lfvectorS(d, r, d, (s/s_prev), numverts);
1020
1021                 filter(d,S);
1022
1023                 conjgrad_loopcount++;
1024         }
1025         // itend();
1026         // printf("cg_filtered time: %f\n", (float)itval());
1027         
1028         conjgrad_lasterror = s;
1029
1030         del_lfvector(q);
1031         del_lfvector(d);
1032         del_lfvector(tmp);
1033         del_lfvector(r);
1034         
1035         // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount);
1036
1037         return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
1038 }
1039
1040 // block diagonalizer
1041 DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1042 {
1043         unsigned int i = 0;
1044         
1045         // Take only the diagonal blocks of A
1046 // #pragma omp parallel for private(i)
1047         for(i = 0; i<lA[0].vcount; i++)
1048         {
1049                 // block diagonalizer
1050                 cp_fmatrix(P[i].m, lA[i].m);
1051                 inverse_fmatrix(Pinv[i].m, P[i].m);
1052                 
1053         }
1054 }
1055
1056 /*
1057 // 1.0 working PCG, but slow and unstable for bigger epsilon + strong forces
1058 int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1059 {
1060         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
1061         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0;
1062         float conjgrad_epsilon=0.01f, conjgrad_lasterror=0;
1063         lfVector *filterb = create_lfvector(numverts);
1064         lfVector *p_fb = create_lfvector(numverts);
1065         lfVector *r = create_lfvector(numverts);
1066         lfVector *c = create_lfvector(numverts);
1067         lfVector *q = create_lfvector(numverts);
1068         lfVector *s = create_lfvector(numverts);
1069         
1070         BuildPPinv(lA, P, Pinv);
1071         
1072         cp_lfvector(dv, z, numverts);
1073         cp_lfvector(filterb, lB, numverts);
1074         filter(filterb, S);
1075         mul_bfmatrix_lfvector(p_fb, P, filterb);
1076         delta0 = dot_lfvector(filterb, p_fb, numverts);
1077         
1078         mul_bfmatrix_lfvector(r, lA, dv);
1079         mul_lfvectorS(r, r, -1.0, numverts);
1080         add_lfvector_lfvector(r, r, lB, numverts);
1081         filter(r, S);
1082         
1083         mul_bfmatrix_lfvector(c, Pinv, r);
1084         filter(c, S);
1085         
1086         deltaNew = dot_lfvector(r, c, numverts);
1087         
1088         do
1089         {
1090                 iterations++;
1091                 
1092                 mul_bfmatrix_lfvector(q, lA, c);
1093                 filter(q, S);
1094                 
1095                 alpha = deltaNew / dot_lfvector(c, q, numverts);
1096                 
1097                 add_lfvector_lfvectorS(dv, dv, c, alpha, numverts);
1098                 
1099                 add_lfvector_lfvectorS(r, r, q, -alpha, numverts);
1100                 
1101                 mul_bfmatrix_lfvector(s, Pinv, r);
1102                 
1103                 
1104                 deltaOld = deltaNew;
1105                 
1106                 deltaNew = dot_lfvector(r, s, numverts);
1107                 
1108                 add_lfvector_lfvectorS(s, s, c, deltaNew / deltaOld, numverts);
1109                 filter(s, S);
1110                 
1111                 cp_lfvector(c, s, numverts);
1112         } while ((deltaNew > (conjgrad_epsilon*conjgrad_epsilon*delta0)) && (iterations < conjgrad_looplimit));
1113         
1114         del_lfvector(s);
1115         del_lfvector(q);
1116         del_lfvector(c);
1117         del_lfvector(r);
1118         del_lfvector(p_fb);
1119         del_lfvector(filterb);
1120         
1121         printf("iterations: %d\n", iterations);
1122         
1123         return iterations<conjgrad_looplimit;
1124 }
1125 */
1126
1127 /*
1128 // 1.1 stable version frmo Box / Aschermann
1129 // but also very slow ;)
1130 int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1131 {
1132         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
1133         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0;
1134         float conjgrad_epsilon=0.01f, conjgrad_lasterror=0;
1135         lfVector *r = create_lfvector(numverts);
1136         lfVector *p = create_lfvector(numverts);
1137         lfVector *s = create_lfvector(numverts);
1138         lfVector *h = create_lfvector(numverts);
1139         
1140         BuildPPinv(lA, P, Pinv);
1141         
1142         cp_lfvector(dv, z, numverts);
1143         
1144         mul_bfmatrix_lfvector(r, lA, dv);
1145         mul_lfvectorS(r, r, -1.0, numverts);
1146         add_lfvector_lfvector(r, r, lB, numverts);
1147         filter(r, S);
1148         
1149         mul_bfmatrix_lfvector(p, Pinv, r);
1150         filter(p, S);
1151         
1152         deltaNew = delta0 = dot_lfvector(r, p, numverts);
1153         
1154         while ((deltaNew > (conjgrad_epsilon*conjgrad_epsilon*delta0)) && (iterations < conjgrad_looplimit))
1155         {
1156                 iterations++;
1157                 
1158                 mul_bfmatrix_lfvector(s, lA, p);
1159                 filter(s, S);
1160                 
1161                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1162                 
1163                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1164                 
1165                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1166                 
1167                 mul_bfmatrix_lfvector(h, Pinv, r);
1168                 
1169                 
1170                 deltaOld = deltaNew;
1171                 
1172                 deltaNew = dot_lfvector(r, h, numverts);
1173                 
1174                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1175                 filter(p, S);
1176                 
1177         }
1178         
1179         del_lfvector(h);
1180         del_lfvector(s);
1181         del_lfvector(p);
1182         del_lfvector(r);
1183         
1184         printf("iterations: %d\n", iterations);
1185         
1186         return iterations<conjgrad_looplimit;
1187 }
1188 */
1189
1190 // 1.3
1191 int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1192 {
1193         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
1194         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0;
1195         float conjgrad_epsilon=0.0001; // 0.2 is dt for steps=5
1196         lfVector *r = create_lfvector(numverts);
1197         lfVector *p = create_lfvector(numverts);
1198         lfVector *s = create_lfvector(numverts);
1199         lfVector *h = create_lfvector(numverts);
1200         
1201         BuildPPinv(lA, P, Pinv);
1202         
1203         filter(dv, S);
1204         add_lfvector_lfvector(dv, dv, z, numverts);
1205         
1206         mul_bfmatrix_lfvector(r, lA, dv);
1207         sub_lfvector_lfvector(r, lB, r, numverts);
1208         filter(r, S);
1209         
1210         mul_bfmatrix_lfvector(p, Pinv, r);
1211         filter(p, S);
1212         
1213         deltaNew = delta0 = dot_lfvector(r, p, numverts);
1214         
1215         // itstart();
1216         
1217         while ((deltaNew > (conjgrad_epsilon*delta0)) && (iterations < conjgrad_looplimit))
1218         {
1219                 iterations++;
1220                 
1221                 mul_bfmatrix_lfvector(s, lA, p);
1222                 filter(s, S);
1223                 
1224                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1225                 
1226                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1227                 
1228                 sub_lfvector_lfvectorS(r, r, s, alpha, numverts);
1229                 
1230                 mul_bfmatrix_lfvector(h, Pinv, r);
1231                 filter(h, S);
1232                 
1233                 deltaOld = deltaNew;
1234                 
1235                 deltaNew = dot_lfvector(r, h, numverts);
1236                 
1237                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1238                 filter(p, S);
1239                 
1240         }
1241         
1242         // itend();
1243         // printf("cg_filtered_pre time: %f\n", (float)itval());
1244         
1245         del_lfvector(h);
1246         del_lfvector(s);
1247         del_lfvector(p);
1248         del_lfvector(r);
1249         
1250         // printf("iterations: %d\n", iterations);
1251         
1252         return iterations<conjgrad_looplimit;
1253 }
1254
1255 // outer product is NOT cross product!!!
1256 DO_INLINE void dfdx_spring_type1(float to[3][4], float dir[3],float length,float L,float k)
1257 {
1258         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1259         // return  (outerprod(dir,dir)*k + (I - outerprod(dir,dir))*(k - ((k*L)/length)));
1260         float temp[3][4];
1261         mul_fvectorT_fvector(temp, dir, dir);
1262         sub_fmatrix_fmatrix(to, I, temp);
1263         mul_fmatrix_S(to, k* (1.0f-(L/length)));
1264         mul_fmatrix_S(temp, k);
1265         add_fmatrix_fmatrix(to, temp, to);
1266 }
1267
1268 DO_INLINE void dfdx_spring_type2(float to[3][4], float dir[3],float length,float L,float k, float cb)
1269 {
1270         // return  outerprod(dir,dir)*fbstar_jacobi(length, L, k, cb);
1271         mul_fvectorT_fvectorS(to, dir, dir, fbstar_jacobi(length, L, k, cb));
1272 }
1273
1274 DO_INLINE void dfdv_damp(float to[3][4], float dir[3], float damping)
1275 {
1276         // derivative of force wrt velocity.  
1277         // return outerprod(dir,dir) * damping;
1278         mul_fvectorT_fvectorS(to, dir, dir, damping);
1279 }
1280
1281 DO_INLINE void dfdx_spring(float to[3][4],  float dir[3],float length,float L,float k)
1282 {
1283         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1284         //return  ( (I-outerprod(dir,dir))*Min(1.0f,rest/length) - I) * -k;
1285         mul_fvectorT_fvector(to, dir, dir);
1286         sub_fmatrix_fmatrix(to, I, to);
1287         mul_fmatrix_S(to, (((L/length)> 1.0f) ? (1.0f): (L/length))); 
1288         sub_fmatrix_fmatrix(to, to, I);
1289         mul_fmatrix_S(to, -k);
1290 }
1291
1292 DO_INLINE void dfdx_damp(float to[3][4],  float dir[3],float length,const float vel[3],float rest,float damping)
1293 {
1294         // inner spring damping   vel is the relative velocity  of the endpoints.  
1295         //      return (I-outerprod(dir,dir)) * (-damping * -(dot(dir,vel)/Max(length,rest)));
1296         mul_fvectorT_fvector(to, dir, dir);
1297         sub_fmatrix_fmatrix(to, I, to);
1298         mul_fmatrix_S(to,  (-damping * -(INPR(dir,vel)/MAX2(length,rest)))); 
1299
1300 }
1301
1302 DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float dt)
1303 {
1304         float extent[3];
1305         float length = 0;
1306         float dir[3] = {0,0,0};
1307         float vel[3] = {0,0,0};
1308         float k = 0.0f;
1309         float L = s->restlen;
1310         float cb = clmd->sim_parms->structural;
1311
1312         float nullf[3] = {0,0,0};
1313         float stretch_force[3] = {0,0,0};
1314         float bending_force[3] = {0,0,0};
1315         float damping_force[3] = {0,0,0};
1316         float nulldfdx[3][4]={ {0,0,0,0}, {0,0,0,0}, {0,0,0,0}};
1317         
1318         VECCOPY(s->f, nullf);
1319         cp_fmatrix(s->dfdx, nulldfdx);
1320         cp_fmatrix(s->dfdv, nulldfdx);
1321
1322         // calculate elonglation
1323         VECSUB(extent, X[s->kl], X[s->ij]);
1324         VECSUB(vel, V[s->kl], V[s->ij]);
1325         length = sqrt(INPR(extent, extent));
1326         
1327         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
1328         
1329         if(length > ABS(ALMOST_ZERO))
1330         {
1331                 /*
1332                 if(length>L)
1333                 {
1334                         if((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) 
1335                         && ((((length-L)*100.0f/L) > clmd->sim_parms->maxspringlen))) // cut spring!
1336                         {
1337                                 s->flags |= CSPRING_FLAG_DEACTIVATE;
1338                                 return;
1339                         }
1340                 } 
1341                 */
1342                 mul_fvector_S(dir, extent, 1.0f/length);
1343         }
1344         else    
1345         {
1346                 mul_fvector_S(dir, extent, 0.0f);
1347         }
1348         
1349         
1350         // calculate force of structural + shear springs
1351         if(s->type != CLOTH_SPRING_TYPE_BENDING)
1352         {
1353                 if(length > L) // only on elonglation
1354                 {
1355                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1356
1357                         k = (clmd->sim_parms->structural*(length-L));   
1358
1359                         mul_fvector_S(stretch_force, dir, k); 
1360
1361                         VECADD(s->f, s->f, stretch_force);
1362
1363                         // Ascher & Boxman, p.21: Damping only during elonglation
1364                         mul_fvector_S(damping_force, extent, clmd->sim_parms->Cdis * 0.01 * ((INPR(vel,extent)/length))); 
1365                         VECADD(s->f, s->f, damping_force);
1366                         
1367                         // Formula from Ascher / Boxman, Speeding up cloth simulation
1368                         // if((dt * (k*dt + 2 * clmd->sim_parms->Cdis * 0.01)) > 0.01 )
1369                         {
1370                                 dfdx_spring_type1(s->dfdx, dir,length,L,clmd->sim_parms->structural);
1371                                 dfdv_damp(s->dfdv, dir,clmd->sim_parms->Cdis * 0.01);
1372                         }
1373                         // printf("(dt*k*dt) ): %f, k: %f\n", (dt * (k*dt + 2 * clmd->sim_parms->Cdis * 0.01) ), k);
1374                 }
1375         }
1376         else // calculate force of bending springs
1377         {
1378                 if(length < L)
1379                 {
1380                         // clmd->sim_parms->flags |= CLOTH_SIMSETTINGS_FLAG_BIG_FORCE;
1381                         
1382                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1383                         
1384                         k = fbstar(length, L, clmd->sim_parms->bending, cb);    
1385
1386                         mul_fvector_S(bending_force, dir, k);
1387                         VECADD(s->f, s->f, bending_force);
1388                         
1389                         // DG: My formula to handle bending for the AIMEX scheme 
1390                         // multiply with 1000 because of numerical problems
1391                         // if( ((k*1000)*dt*dt) < -0.18 )
1392                         {
1393                                 dfdx_spring_type2(s->dfdx, dir,length,L,clmd->sim_parms->bending, cb);
1394                                 clmd->sim_parms->flags |= CLOTH_SIMSETTINGS_FLAG_BIG_FORCE;
1395                         }
1396                         // printf("(dt*k*dt) ): %f, k: %f\n", (dt*dt*k*-1.0), k);
1397                 }
1398         }
1399 }
1400
1401 DO_INLINE int cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1402 {
1403         if(s->flags & CLOTH_SPRING_FLAG_NEEDED)
1404         {
1405                 VECADD(lF[s->ij], lF[s->ij], s->f);
1406                 VECSUB(lF[s->kl], lF[s->kl], s->f);     
1407                         
1408                 if(s->type != CLOTH_SPRING_TYPE_BENDING)
1409                 {
1410                         sub_fmatrix_fmatrix(dFdV[s->ij].m, dFdV[s->ij].m, s->dfdv);
1411                         sub_fmatrix_fmatrix(dFdV[s->kl].m, dFdV[s->kl].m, s->dfdv);
1412                         add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv); 
1413                 }
1414                 else if(!(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_BIG_FORCE))
1415                         return 0;
1416                 
1417                 sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx);
1418                 sub_fmatrix_fmatrix(dFdX[s->kl].m, dFdX[s->kl].m, s->dfdx);
1419
1420                 add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx);
1421         }
1422         
1423         return 1;
1424 }
1425
1426 DO_INLINE void calculateTriangleNormal(float to[3], lfVector *X, MFace mface)
1427 {
1428         float v1[3], v2[3];
1429
1430         VECSUB(v1, X[mface.v2], X[mface.v1]);
1431         VECSUB(v2, X[mface.v3], X[mface.v1]);
1432         cross_fvector(to, v1, v2);
1433 }
1434
1435 DO_INLINE void calculatQuadNormal(float to[3], lfVector *X, MFace mface)
1436 {
1437         float temp = CalcNormFloat4(X[mface.v1],X[mface.v2],X[mface.v3],X[mface.v4],to);
1438         mul_fvector_S(to, to, temp);
1439 }
1440
1441 void calculateWeightedVertexNormal(ClothModifierData *clmd, MFace *mfaces, float to[3], int index, lfVector *X)
1442 {
1443         float temp[3]; 
1444         int i;
1445         Cloth *cloth = clmd->clothObject;
1446
1447         for(i = 0; i < cloth->numfaces; i++)
1448         {
1449                 // check if this triangle contains the selected vertex
1450                 if(mfaces[i].v1 == index || mfaces[i].v2 == index || mfaces[i].v3 == index || mfaces[i].v4 == index)
1451                 {
1452                         calculatQuadNormal(temp, X, mfaces[i]);
1453                         VECADD(to, to, temp);
1454                 }
1455         }
1456 }
1457 float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
1458 {
1459         return fabs(INPR(wind, vertexnormal) * 0.5f);
1460 }
1461
1462 DO_INLINE void calc_triangle_force(ClothModifierData *clmd, MFace mface, lfVector *F, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors)
1463 {       
1464
1465 }
1466
1467 void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVector *lV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors, float time, float dt)
1468 {
1469         /* Collect forces and derivatives:  F,dFdX,dFdV */
1470         Cloth           *cloth          = clmd->clothObject;
1471         unsigned int    i               = 0;
1472         float           spring_air      = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */
1473         float           gravity[3];
1474         float           tm2[3][4]       = {{-spring_air,0,0,0}, {0,-spring_air,0,0},{0,0,-spring_air,0}};
1475         ClothVertex *verts = cloth->verts;
1476         MFace           *mfaces         = cloth->mfaces;
1477         float wind_normalized[3];
1478         unsigned int numverts = cloth->numverts;
1479         float auxvect[3], velgoal[3], tvect[3];
1480         float kd, ks;
1481         LinkNode *search = cloth->springs;
1482
1483         VECCOPY(gravity, clmd->sim_parms->gravity);
1484         mul_fvector_S(gravity, gravity, 0.001f); /* scale gravity force */
1485
1486         /* set dFdX jacobi matrix to zero */
1487         init_bfmatrix(dFdX, ZERO);
1488         /* set dFdX jacobi matrix diagonal entries to -spring_air */ 
1489         initdiag_bfmatrix(dFdV, tm2);
1490
1491         init_lfvector(lF, gravity, numverts);
1492
1493         submul_lfvectorS(lF, lV, spring_air, numverts);
1494
1495         /* do goal stuff */
1496         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
1497         {       
1498                 for(i = 0; i < numverts; i++)
1499                 {                       
1500                         if(verts [i].goal < SOFTGOALSNAP)
1501                         {                       
1502                                 // current_position = xold + t * (newposition - xold)
1503                                 VECSUB(tvect, cloth->xconst[i], cloth->xold[i]);
1504                                 mul_fvector_S(tvect, tvect, time);
1505                                 VECADD(tvect, tvect, cloth->xold[i]);
1506
1507                                 VECSUB(auxvect, tvect, lX[i]);
1508                                 ks  = 1.0f/(1.0f- verts [i].goal*clmd->sim_parms->goalspring)-1.0f ;
1509                                 VECADDS(lF[i], lF[i], auxvect, -ks);
1510
1511                                 // calulate damping forces generated by goals                           
1512                                 VECSUB(velgoal, cloth->xold[i], cloth->xconst[i]);
1513                                 kd =  clmd->sim_parms->goalfrict * 0.01f; // friction force scale taken from SB
1514                                 VECSUBADDSS(lF[i], velgoal, kd, lV[i], kd);
1515                                 
1516                         }
1517                 }       
1518         }
1519
1520         /* handle external forces like wind */
1521         if(effectors)
1522         {
1523                 float speed[3] = {0.0f, 0.0f,0.0f};
1524                 float force[3]= {0.0f, 0.0f, 0.0f};
1525                 
1526                 #pragma omp parallel for private (i) shared(lF)
1527                 for(i = 0; i < cloth->numverts; i++)
1528                 {
1529                         float vertexnormal[3]={0,0,0};
1530                         float fieldfactor = 1000.0f, windfactor  = 250.0f; // from sb
1531                         
1532                         pdDoEffectors(effectors, lX[i], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);          
1533                         
1534                         // TODO apply forcefields here
1535                         VECADDS(lF[i], lF[i], force, fieldfactor*0.01f);
1536
1537                         VECCOPY(wind_normalized, speed);
1538                         Normalize(wind_normalized);
1539                         
1540                         calculateWeightedVertexNormal(clmd, mfaces, vertexnormal, i, lX);
1541                         VECADDS(lF[i], lF[i], wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
1542                 }
1543         }
1544         
1545         // calculate spring forces
1546         search = cloth->springs;
1547         while(search)
1548         {
1549                 // only handle active springs
1550                 // if(((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED)){}
1551                 cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX, dt);
1552
1553                 search = search->next;
1554         }
1555         
1556         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_BIG_FORCE)
1557         {       
1558                 if(cloth->numspringssave != cloth->numsprings)
1559                 {
1560                         cloth_bending_mode(clmd, 1);
1561                 }
1562         }
1563         else
1564         {
1565                 if(cloth->numspringssave == cloth->numsprings)
1566                 {
1567                         cloth_bending_mode(clmd, 0);
1568                 }
1569         }
1570         
1571         // apply spring forces
1572         search = cloth->springs;
1573         while(search)
1574         {
1575                 // only handle active springs
1576                 // if(((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED))  
1577                 if(!cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX))
1578                         break;
1579                 search = search->next;
1580         }
1581         
1582         clmd->sim_parms->flags &= ~CLOTH_SIMSETTINGS_FLAG_BIG_FORCE;
1583         
1584 }
1585
1586
1587 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, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1588 {
1589         unsigned int numverts = dFdV[0].vcount;
1590
1591         lfVector *dFdXmV = create_lfvector(numverts);
1592         
1593         initdiag_bfmatrix(A, I);
1594         
1595         subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt));
1596
1597         mul_bfmatrix_lfvector(dFdXmV, dFdX, lV);
1598
1599         add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, (dt*dt), numverts);
1600         
1601         cg_filtered(dV, A, B, z, S); // conjugate gradient algorithm to solve Ax=b 
1602         // cg_filtered_pre(dV, A, B, z, S, P, Pinv);
1603         
1604         // advance velocities
1605         add_lfvector_lfvector(Vnew, lV, dV, numverts);
1606         
1607         del_lfvector(dFdXmV);
1608 }
1609
1610 /*
1611 // this version solves for the new velocity
1612 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, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1613 {
1614         unsigned int numverts = dFdV[0].vcount;
1615
1616         lfVector *dFdXmV = create_lfvector(numverts);
1617         
1618         initdiag_bfmatrix(A, I);
1619         
1620         subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt));
1621
1622         mul_bfmatrix_lfvector(dFdXmV, dFdV, lV);
1623
1624         add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, -dt, numverts);
1625         add_lfvector_lfvector(B, B, lV, numverts);
1626
1627         cg_filtered_pre(Vnew, A, B, z, S, P, Pinv);
1628         
1629         del_lfvector(dFdXmV);
1630 }
1631 */
1632 int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
1633 {               
1634         unsigned int i=0;
1635         float step=0.0f, tf=1.0f;
1636         Cloth *cloth = clmd->clothObject;
1637         ClothVertex *verts = cloth->verts;
1638         unsigned int numverts = cloth->numverts;
1639         float dt = 1.0f / clmd->sim_parms->stepsPerFrame;
1640         Implicit_Data *id = cloth->implicit;
1641         int result = 0;
1642         float force = 0, lastforce = 0;
1643         lfVector *dx;
1644         
1645         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1646         {
1647                 for(i = 0; i < numverts; i++)
1648                 {                       
1649                         // update velocities with constrained velocities from pinned verts
1650                         if(verts [i].goal >= SOFTGOALSNAP)
1651                         {                       
1652                                 VECSUB(id->V[i], cloth->xconst[i], cloth->xold[i]);
1653                                 // VecMulf(id->V[i], 1.0 / dt);
1654                         }
1655                 }       
1656         }
1657
1658         while(step < tf)
1659         {               
1660                 effectors= pdInitEffectors(ob,NULL);
1661                 
1662                 // calculate 
1663                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, dt );
1664                 
1665                 // check for sleeping
1666                 // if(!(clmd->coll_parms->flags & CLOTH_SIMSETTINGS_FLAG_SLEEP))
1667                 {
1668                         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->P, id->Pinv);
1669                 
1670                         add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
1671                 }
1672                 
1673                 dx = create_lfvector(numverts);
1674                 sub_lfvector_lfvector(dx, id->Xnew, id->X, numverts);
1675                 force = dot_lfvector(dx, dx, numverts);
1676                 del_lfvector(dx);
1677                 
1678                 /*
1679                 if((force < 0.00001) && (lastforce >= force))
1680                         clmd->coll_parms->flags |= CLOTH_SIMSETTINGS_FLAG_SLEEP;
1681                 else if((lastforce*2 < force))
1682                 */
1683                         clmd->coll_parms->flags &= ~CLOTH_SIMSETTINGS_FLAG_SLEEP;
1684                 
1685                 lastforce = force;
1686                 
1687                 if(clmd->coll_parms->flags & CLOTH_COLLISIONSETTINGS_FLAG_ENABLED)
1688                 {
1689                         // collisions 
1690                         // itstart();
1691                         
1692                         // update verts to current positions
1693                         for(i = 0; i < numverts; i++)
1694                         {               
1695                                 if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1696                                 {                       
1697                                         if(verts [i].goal >= SOFTGOALSNAP)
1698                                         {                       
1699                                                 float tvect[3] = {.0,.0,.0};
1700                                                 // VECSUB(tvect, id->Xnew[i], verts[i].xold);
1701                                                 mul_fvector_S(tvect, id->V[i], step+dt);
1702                                                 VECADD(tvect, tvect, cloth->xold[i]);
1703                                                 VECCOPY(id->Xnew[i], tvect);
1704                                         }
1705                                                 
1706                                 }
1707                                 
1708                                 VECCOPY(cloth->current_x[i], id->Xnew[i]);
1709                                 VECSUB(cloth->current_v[i], cloth->current_x[i], cloth->current_xold[i]);
1710                                 VECCOPY(cloth->v[i], cloth->current_v[i]);
1711                         }
1712                         
1713                         // call collision function
1714                         result = cloth_bvh_objcollision(clmd, step + dt, step, dt);
1715                         
1716                         // copy corrected positions back to simulation
1717                         if(result)
1718                         {
1719                                 memcpy(cloth->current_xold, cloth->current_x, sizeof(lfVector) * numverts);
1720                                 memcpy(id->Xnew, cloth->current_x, sizeof(lfVector) * numverts);
1721                                 
1722                                 for(i = 0; i < numverts; i++)
1723                                 {       
1724                                         VECCOPY(id->Vnew[i], cloth->current_v[i]);
1725                                         VecMulf(id->Vnew[i], 1.0f / dt);
1726                                 }
1727                         }
1728                         else
1729                         {
1730                                 memcpy(cloth->current_xold, id->Xnew, sizeof(lfVector) * numverts);
1731                         }
1732                         
1733                         // X = Xnew;
1734                         cp_lfvector(id->X, id->Xnew, numverts);
1735                         
1736                         // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
1737                         if(result)
1738                         {
1739                                 // V = Vnew;
1740                                 cp_lfvector(id->V, id->Vnew, numverts);
1741                                 
1742                                 // calculate 
1743                                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, dt);   
1744                                 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->P, id->Pinv);
1745                         }
1746                         
1747                 }
1748                 else
1749                 {
1750                         // X = Xnew;
1751                         cp_lfvector(id->X, id->Xnew, numverts);
1752                 }
1753                 
1754                 // itend();
1755                 // printf("collision time: %f\n", (float)itval());
1756                 
1757                 // V = Vnew;
1758                 cp_lfvector(id->V, id->Vnew, numverts);
1759                 
1760                 step += dt;
1761
1762                 if(effectors) pdEndEffectors(effectors);
1763         }
1764         
1765         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL)
1766         {
1767                 for(i = 0; i < numverts; i++)
1768                 {
1769                         if(verts [i].goal < SOFTGOALSNAP)
1770                         {
1771                                 VECCOPY(cloth->current_xold[i], id->X[i]);
1772                                 VECCOPY(cloth->x[i], id->X[i]);
1773                         }
1774                         else
1775                         {
1776                                 VECCOPY(cloth->current_xold[i], cloth->xconst[i]);
1777                                 VECCOPY(cloth->x[i], cloth->xconst[i]);
1778                         }
1779                 }
1780         }
1781         else
1782         {
1783                 for(i = 0; i < numverts; i++)
1784                 {
1785                         VECCOPY(cloth->current_xold[i], id->X[i]);
1786                         VECCOPY(cloth->x[i], id->X[i]);
1787                 }
1788                 // memcpy(cloth->current_xold, id->X, sizeof(lfVector) * numverts);
1789                 // memcpy(cloth->x, id->X, sizeof(lfVector) * numverts);
1790         }
1791         
1792         for(i = 0; i < numverts; i++)
1793                 VECCOPY(cloth->v[i], id->V[i]);
1794         
1795         // memcpy(cloth->v, id->V, sizeof(lfVector) * numverts);
1796         
1797         return 1;
1798 }
1799
1800 void implicit_set_positions (ClothModifierData *clmd)
1801 {               
1802         Cloth *cloth = clmd->clothObject;
1803         unsigned int numverts = cloth->numverts, i = 0;
1804         Implicit_Data *id = cloth->implicit;
1805         
1806         
1807         for(i = 0; i < numverts; i++)
1808         {
1809                 VECCOPY(id->X[i], cloth->x[i]);
1810                 VECCOPY(id->V[i], cloth->v[i]);
1811         }
1812         
1813         // memcpy(id->X, cloth->x, sizeof(lfVector) * numverts);        
1814         // memcpy(id->V, cloth->v, sizeof(lfVector) * numverts);        
1815 }
1816
1817
1818 int collisions_collision_response_static(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1819 {
1820         /*
1821         unsigned int i = 0;
1822         int result = 0;
1823         LinkNode *search = NULL;
1824         CollPair *collpair = NULL;
1825         Cloth *cloth1, *cloth2;
1826         float w1, w2, w3, u1, u2, u3;
1827         float v1[3], v2[3], relativeVelocity[3];
1828         float magrelVel;
1829         
1830         cloth1 = clmd->clothObject;
1831         cloth2 = coll_clmd->clothObject;
1832
1833         // search = clmd->coll_parms->collision_list;
1834         
1835         while(search)
1836         {
1837         collpair = search->link;
1838                 
1839                 // compute barycentric coordinates for both collision points
1840         collisions_compute_barycentric(collpair->pa,
1841         cloth1->verts[collpair->ap1].txold,
1842         cloth1->verts[collpair->ap2].txold,
1843         cloth1->verts[collpair->ap3].txold, 
1844         &w1, &w2, &w3);
1845         
1846         collisions_compute_barycentric(collpair->pb,
1847         cloth2->verts[collpair->bp1].txold,
1848         cloth2->verts[collpair->bp2].txold,
1849         cloth2->verts[collpair->bp3].txold,
1850         &u1, &u2, &u3);
1851         
1852                 // Calculate relative "velocity".
1853         interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
1854                 
1855         interpolateOnTriangle(v2, cloth2->verts[collpair->bp1].tv, cloth2->verts[collpair->bp2].tv, cloth2->verts[collpair->bp3].tv, u1, u2, u3);
1856                 
1857         VECSUB(relativeVelocity, v1, v2);
1858                         
1859                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
1860         magrelVel = INPR(relativeVelocity, collpair->normal);
1861                 
1862                 // printf("magrelVel: %f\n", magrelVel);
1863                                 
1864                 // Calculate masses of points.
1865                 
1866                 // If v_n_mag < 0 the edges are approaching each other.
1867         if(magrelVel < -ALMOST_ZERO) 
1868         {
1869                         // Calculate Impulse magnitude to stop all motion in normal direction.
1870                         // const double I_mag = v_n_mag / (1/m1 + 1/m2);
1871         float magnitude_i = magrelVel / 2.0f; // TODO implement masses
1872         float tangential[3], magtangent, magnormal, collvel[3];
1873         float vrel_t_pre[3];
1874         float vrel_t[3];
1875         double impulse;
1876         float epsilon = clmd->coll_parms->epsilon;
1877         float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
1878                         
1879                         // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms->friction*0.01, magrelVel);
1880                         
1881                         // magtangent = INPR(tangential, tangential);
1882                         
1883                         // Apply friction impulse.
1884         if (magtangent < -ALMOST_ZERO) 
1885         {
1886                                 
1887                                 // printf("friction applied: %f\n", magtangent);
1888                                 // TODO check original code 
1889 }
1890                         
1891
1892         impulse = -2.0f * magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
1893                         
1894                         // printf("impulse: %f\n", impulse);
1895                         
1896                         // face A
1897         VECADDMUL(cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse); 
1898         cloth1->verts[collpair->ap1].impulse_count++;
1899                         
1900         VECADDMUL(cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse); 
1901         cloth1->verts[collpair->ap2].impulse_count++;
1902                         
1903         VECADDMUL(cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse); 
1904         cloth1->verts[collpair->ap3].impulse_count++;
1905                         
1906                         // face B
1907         VECADDMUL(cloth2->verts[collpair->bp1].impulse, collpair->normal, u1 * impulse); 
1908         cloth2->verts[collpair->bp1].impulse_count++;
1909                         
1910         VECADDMUL(cloth2->verts[collpair->bp2].impulse, collpair->normal, u2 * impulse); 
1911         cloth2->verts[collpair->bp2].impulse_count++;
1912                         
1913         VECADDMUL(cloth2->verts[collpair->bp3].impulse, collpair->normal, u3 * impulse); 
1914         cloth2->verts[collpair->bp3].impulse_count++;
1915                         
1916                         
1917         result = 1;     
1918                 
1919                         // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
1920                         
1921                         // Apply the impulse and increase impulse counters.
1922         
1923                         
1924 }
1925                 
1926         search = search->next;
1927 }
1928         
1929         
1930         return result;
1931         */
1932         return 0;
1933 }
1934
1935
1936 int collisions_collision_response_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1937 {
1938         return 0;
1939 }
1940
1941
1942 int collisions_collision_response_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1943 {
1944         return 0;
1945 }
1946
1947 void cloth_collision_static(ClothModifierData *clmd, LinkNode *collision_list)
1948 {
1949         /*
1950         CollPair *collpair = NULL;
1951         Cloth *cloth1=NULL, *cloth2=NULL;
1952         MFace *face1=NULL, *face2=NULL;
1953         ClothVertex *verts1=NULL, *verts2=NULL;
1954         double distance = 0;
1955         float epsilon = clmd->coll_parms->epsilon;
1956         unsigned int i = 0;
1957
1958         for(i = 0; i < 4; i++)
1959         {
1960         collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");        
1961                 
1962         cloth1 = clmd->clothObject;
1963         cloth2 = coll_clmd->clothObject;
1964                 
1965         verts1 = cloth1->verts;
1966         verts2 = cloth2->verts;
1967         
1968         face1 = &(cloth1->mfaces[tree1->tri_index]);
1969         face2 = &(cloth2->mfaces[tree2->tri_index]);
1970                 
1971                 // check all possible pairs of triangles
1972         if(i == 0)
1973         {
1974         collpair->ap1 = face1->v1;
1975         collpair->ap2 = face1->v2;
1976         collpair->ap3 = face1->v3;
1977                         
1978         collpair->bp1 = face2->v1;
1979         collpair->bp2 = face2->v2;
1980         collpair->bp3 = face2->v3;
1981                         
1982 }
1983                 
1984         if(i == 1)
1985         {
1986         if(face1->v4)
1987         {
1988         collpair->ap1 = face1->v3;
1989         collpair->ap2 = face1->v4;
1990         collpair->ap3 = face1->v1;
1991                                 
1992         collpair->bp1 = face2->v1;
1993         collpair->bp2 = face2->v2;
1994         collpair->bp3 = face2->v3;
1995 }
1996         else
1997         i++;
1998 }
1999                 
2000         if(i == 2)
2001         {
2002         if(face2->v4)
2003         {
2004         collpair->ap1 = face1->v1;
2005         collpair->ap2 = face1->v2;
2006         collpair->ap3 = face1->v3;
2007                                 
2008         collpair->bp1 = face2->v3;
2009         collpair->bp2 = face2->v4;
2010         collpair->bp3 = face2->v1;
2011 }
2012         else
2013         i+=2;
2014 }
2015                 
2016         if(i == 3)
2017         {
2018         if((face1->v4)&&(face2->v4))
2019         {
2020         collpair->ap1 = face1->v3;
2021         collpair->ap2 = face1->v4;
2022         collpair->ap3 = face1->v1;
2023                                 
2024         collpair->bp1 = face2->v3;
2025         collpair->bp2 = face2->v4;
2026         collpair->bp3 = face2->v1;
2027 }
2028         else
2029         i++;
2030 }
2031         
2032                 
2033         if(i < 4)
2034         {
2035                         // calc distance + normal       
2036         distance = plNearestPoints(
2037         verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, verts2[collpair->bp1].txold, verts2[collpair->bp2].txold, verts2[collpair->bp3].txold, collpair->pa,collpair->pb,collpair->vector);
2038                         
2039         if (distance <= (epsilon + ALMOST_ZERO))
2040         {
2041                                 // printf("dist: %f\n", (float)distance);
2042                                 
2043                                 // collpair->face1 = tree1->tri_index;
2044                                 // collpair->face2 = tree2->tri_index;
2045                                 
2046                                 // VECCOPY(collpair->normal, collpair->vector);
2047                                 // Normalize(collpair->normal);
2048                                 
2049                                 // collpair->distance = distance;
2050                                 
2051 }
2052         else
2053         {
2054         MEM_freeN(collpair);
2055 }
2056 }
2057         else
2058         {
2059         MEM_freeN(collpair);
2060 }
2061 }
2062         */
2063 }
2064
2065 int collisions_are_edges_adjacent(ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair)
2066 {
2067         Cloth *cloth1, *cloth2;
2068         ClothVertex *verts1, *verts2;
2069         float temp[3];
2070          /*
2071         cloth1 = clmd->clothObject;
2072         cloth2 = coll_clmd->clothObject;
2073         
2074         verts1 = cloth1->verts;
2075         verts2 = cloth2->verts;
2076         
2077         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold);
2078         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
2079                 return 1;
2080         
2081         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold);
2082         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
2083                 return 1;
2084         
2085         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold);
2086         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
2087                 return 1;
2088         
2089         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold);
2090         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
2091                 return 1;
2092                 */
2093         return 0;
2094 }
2095
2096
2097 void collisions_collision_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
2098 {
2099         /*
2100         EdgeCollPair edgecollpair;
2101         Cloth *cloth1=NULL, *cloth2=NULL;
2102         MFace *face1=NULL, *face2=NULL;
2103         ClothVertex *verts1=NULL, *verts2=NULL;
2104         double distance = 0;
2105         float epsilon = clmd->coll_parms->epsilon;
2106         unsigned int i = 0, j = 0, k = 0;
2107         int numsolutions = 0;
2108         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
2109         
2110         cloth1 = clmd->clothObject;
2111         cloth2 = coll_clmd->clothObject;
2112         
2113         verts1 = cloth1->verts;
2114         verts2 = cloth2->verts;
2115
2116         face1 = &(cloth1->mfaces[tree1->tri_index]);
2117         face2 = &(cloth2->mfaces[tree2->tri_index]);
2118         
2119         for( i = 0; i < 5; i++)
2120         {
2121         if(i == 0) 
2122         {
2123         edgecollpair.p11 = face1->v1;
2124         edgecollpair.p12 = face1->v2;
2125 }
2126         else if(i == 1) 
2127         {
2128         edgecollpair.p11 = face1->v2;
2129         edgecollpair.p12 = face1->v3;
2130 }
2131         else if(i == 2) 
2132         {
2133         if(face1->v4) 
2134         {
2135         edgecollpair.p11 = face1->v3;
2136         edgecollpair.p12 = face1->v4;
2137 }
2138         else 
2139         {
2140         edgecollpair.p11 = face1->v3;
2141         edgecollpair.p12 = face1->v1;
2142         i+=5; // get out of here after this edge pair is handled
2143 }
2144 }
2145         else if(i == 3) 
2146         {
2147         if(face1->v4) 
2148         {
2149         edgecollpair.p11 = face1->v4;
2150         edgecollpair.p12 = face1->v1;
2151 }       
2152         else
2153         continue;
2154 }
2155         else
2156         {
2157         edgecollpair.p11 = face1->v3;
2158         edgecollpair.p12 = face1->v1;
2159 }
2160
2161                 
2162         for( j = 0; j < 5; j++)
2163         {
2164         if(j == 0)
2165         {
2166         edgecollpair.p21 = face2->v1;
2167         edgecollpair.p22 = face2->v2;
2168 }
2169         else if(j == 1)
2170         {
2171         edgecollpair.p21 = face2->v2;
2172         edgecollpair.p22 = face2->v3;
2173 }
2174         else if(j == 2)
2175         {
2176         if(face2->v4) 
2177         {
2178         edgecollpair.p21 = face2->v3;
2179         edgecollpair.p22 = face2->v4;
2180 }
2181         else 
2182         {
2183         edgecollpair.p21 = face2->v3;
2184         edgecollpair.p22 = face2->v1;
2185 }
2186 }
2187         else if(j == 3)
2188         {
2189         if(face2->v4) 
2190         {
2191         edgecollpair.p21 = face2->v4;
2192         edgecollpair.p22 = face2->v1;
2193 }
2194         else
2195         continue;
2196 }
2197         else
2198         {
2199         edgecollpair.p21 = face2->v3;
2200         edgecollpair.p22 = face2->v1;
2201 }
2202                         
2203                         
2204         if(!collisions_are_edges_adjacent(clmd, coll_clmd, &edgecollpair))
2205         {
2206         VECSUB(a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold);
2207         VECSUB(b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v);
2208         VECSUB(c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold);
2209         VECSUB(d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v);
2210         VECSUB(e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold);
2211         VECSUB(f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v);
2212                                 
2213         numsolutions = collisions_get_collision_time(a, b, c, d, e, f, solution);
2214                                 
2215         for (k = 0; k < numsolutions; k++) 
2216         {                                                               
2217         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
2218         {
2219         float out_collisionTime = solution[k];
2220                                                 
2221                                                 // TODO: check for collisions 
2222                                                 
2223                                                 // TODO: put into (edge) collision list
2224                                                 
2225         printf("Moving edge found!\n");
2226 }
2227 }
2228 }
2229 }
2230 }       
2231         */      
2232 }
2233
2234 void collisions_collision_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
2235 {
2236         /*
2237         CollPair collpair;
2238         Cloth *cloth1=NULL, *cloth2=NULL;
2239         MFace *face1=NULL, *face2=NULL;
2240         ClothVertex *verts1=NULL, *verts2=NULL;
2241         double distance = 0;
2242         float epsilon = clmd->coll_parms->epsilon;
2243         unsigned int i = 0, j = 0, k = 0;
2244         int numsolutions = 0;
2245         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
2246
2247         for(i = 0; i < 2; i++)
2248         {               
2249         cloth1 = clmd->clothObject;
2250         cloth2 = coll_clmd->clothObject;
2251                 
2252         verts1 = cloth1->verts;
2253         verts2 = cloth2->verts;
2254         
2255         face1 = &(cloth1->mfaces[tree1->tri_index]);
2256         face2 = &(cloth2->mfaces[tree2->tri_index]);
2257                 
2258                 // check all possible pairs of triangles
2259         if(i == 0)
2260         {
2261         collpair.ap1 = face1->v1;
2262         collpair.ap2 = face1->v2;
2263         collpair.ap3 = face1->v3;
2264                         
2265         collpair.pointsb[0] = face2->v1;
2266         collpair.pointsb[1] = face2->v2;
2267         collpair.pointsb[2] = face2->v3;
2268         collpair.pointsb[3] = face2->v4;
2269 }
2270                 
2271         if(i == 1)
2272         {
2273         if(face1->v4)
2274         {
2275         collpair.ap1 = face1->v3;
2276         collpair.ap2 = face1->v4;
2277         collpair.ap3 = face1->v1;
2278                                 
2279         collpair.pointsb[0] = face2->v1;
2280         collpair.pointsb[1] = face2->v2;
2281         collpair.pointsb[2] = face2->v3;
2282         collpair.pointsb[3] = face2->v4;
2283 }
2284         else
2285         i++;
2286 }
2287                 
2288                 // calc SIPcode (?)
2289                 
2290         if(i < 2)
2291         {
2292         VECSUB(a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold);
2293         VECSUB(b, verts1[collpair.ap2].v, verts1[collpair.ap1].v);
2294         VECSUB(c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold);
2295         VECSUB(d, verts1[collpair.ap3].v, verts1[collpair.ap1].v);
2296                                 
2297         for(j = 0; j < 4; j++)
2298         {                                       
2299         if((j==3) && !(face2->v4))
2300         break;
2301                                 
2302         VECSUB(e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold);
2303         VECSUB(f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v);
2304                                 
2305         numsolutions = collisions_get_collision_time(a, b, c, d, e, f, solution);
2306                                 
2307         for (k = 0; k < numsolutions; k++) 
2308         {                                                               
2309         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
2310         {
2311         float out_collisionTime = solution[k];
2312                                                 
2313                                                 // TODO: check for collisions 
2314                                                 
2315                                                 // TODO: put into (point-face) collision list
2316                                                 
2317         printf("Moving found!\n");
2318                                                 
2319 }
2320 }
2321                                 
2322                                 // TODO: check borders for collisions
2323 }
2324                         
2325 }
2326 }
2327         */
2328 }
2329
2330 void collisions_collision_moving(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
2331 {
2332         /*
2333         // TODO: check for adjacent
2334         collisions_collision_moving_edges(clmd, coll_clmd, tree1, tree2);
2335         
2336         collisions_collision_moving_tris(clmd, coll_clmd, tree1, tree2);
2337         collisions_collision_moving_tris(coll_clmd, clmd, tree2, tree1);
2338         */
2339 }
2340
2341 // cloth - object collisions
2342 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, float prevstep, float dt)
2343 {
2344         
2345         Base *base = NULL;
2346         CollisionModifierData *collmd = NULL;
2347         Cloth *cloth = NULL;
2348         Object *ob2 = NULL;
2349         BVH *bvh1 = NULL, *bvh2 = NULL, *self_bvh;
2350         LinkNode *collision_list = NULL; 
2351         unsigned int i = 0, j = 0;
2352         int collisions = 0, count = 0;
2353         float (*current_x)[4];
2354
2355         if (!(((Cloth *)clmd->clothObject)->tree))
2356         {
2357                 printf("No BVH found\n");
2358                 return 0;
2359         }
2360         
2361         cloth = clmd->clothObject;
2362         bvh1 = cloth->tree;
2363         self_bvh = cloth->selftree;
2364         
2365         ////////////////////////////////////////////////////////////
2366         // static collisions
2367         ////////////////////////////////////////////////////////////
2368         
2369         // update cloth bvh
2370         bvh_update_from_float4(bvh1, cloth->current_xold, cloth->numverts, cloth->current_x, 0); // 0 means STATIC, 1 means MOVING (see later in this function)
2371 /*
2372         // check all collision objects
2373         for (base = G.scene->base.first; base; base = base->next)
2374         {
2375                 ob2 = base->object;
2376                 collmd = (CollisionModifierData *) modifiers_findByType (ob2, eModifierType_Collision);
2377                 
2378                 if (!collmd)
2379                         continue;
2380                 
2381                 // check if there is a bounding volume hierarchy
2382                 if (collmd->tree) 
2383                 {                       
2384                         bvh2 = collmd->tree;
2385                         
2386                         // update position + bvh of collision object
2387                         collision_move_object(collmd, step, prevstep);
2388                         bvh_update_from_mvert(collmd->tree, collmd->current_x, collmd->numverts, NULL, 0);
2389                         
2390                         // fill collision list 
2391                         collisions += bvh_traverse(bvh1->root, bvh2->root, &collision_list);
2392                         
2393                         // call static collision response
2394                         
2395                         // free collision list
2396                         if(collision_list)
2397                         {
2398                                 LinkNode *search = collision_list;
2399                                 
2400                                 while(search)
2401                                 {
2402                                         CollisionPair *coll_pair = search->link;
2403                                         
2404                                         MEM_freeN(coll_pair);
2405                                         search = search->next;
2406                                 }
2407                                 BLI_linklist_free(collision_list,NULL);
2408         
2409                                 collision_list = NULL;
2410                         }
2411                 }
2412         }
2413         
2414         //////////////////////////////////////////////
2415         // update velocities + positions
2416         //////////////////////////////////////////////
2417         for(i = 0; i < cloth->numverts; i++)
2418         {
2419                 VECADD(cloth->current_x[i], cloth->current_xold[i], cloth->current_v[i]);
2420         }
2421         //////////////////////////////////////////////
2422 */      
2423         /*
2424         // fill collision list 
2425         collisions += bvh_traverse(self_bvh->root, self_bvh->root, &collision_list);
2426         
2427         // call static collision response
2428         
2429         // free collision list
2430         if(collision_list)
2431         {
2432                 LinkNode *search = collision_list;
2433                 
2434                 while(search)
2435                 {
2436                         float distance = 0;
2437                         float mindistance = cloth->selftree->epsilon;
2438                         CollisionPair *collpair = (CollisionPair *)search->link;
2439                         
2440                         // get distance of faces
2441                         distance = plNearestPoints(
2442                                         cloth->current_x[collpair->point_indexA[0]], cloth->current_x[collpair->point_indexA[1]], cloth->current_x[collpair->point_indexA[2]], cloth->current_x[collpair->point_indexB[0]], cloth->current_x[collpair->point_indexB[1]], cloth->current_x[collpair->point_indexB[2]], collpair->pa,collpair->pb,collpair->vector);
2443                                         
2444                         if(distance < mindistance)
2445                         {
2446                                 ///////////////////////////////////////////
2447                                 // TODO: take velocity of the collision points into account!
2448                                 ///////////////////////////////////////////
2449                                 
2450                                 float correction = mindistance - distance;
2451                                 float temp[3];
2452                                 
2453                                 VECCOPY(temp, collpair->vector);
2454                                 Normalize(temp);
2455                                 VecMulf(temp, -correction*0.5);
2456                                 
2457                                 if(!((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [collpair->point_indexA[0]].goal >= SOFTGOALSNAP)))
2458                                         VECSUB(cloth->current_x[collpair->point_indexA[0]], cloth->current_x[collpair->point_indexA[0]], temp); 
2459                                 
2460                                 if(!((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [collpair->point_indexA[1]].goal >= SOFTGOALSNAP)))
2461                                         VECSUB(cloth->current_x[collpair->point_indexA[1]], cloth->current_x[collpair->point_indexA[1]], temp);
2462                                 
2463                                 if(!((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [collpair->point_indexA[2]].goal >= SOFTGOALSNAP)))
2464                                         VECSUB(cloth->current_x[collpair->point_indexA[2]], cloth->current_x[collpair->point_indexA[2]], temp);
2465                                 
2466                                 
2467                                 if(!((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [collpair->point_indexB[0]].goal >= SOFTGOALSNAP)))
2468                                         VECSUB(cloth->current_x[collpair->point_indexB[0]], cloth->current_x[collpair->point_indexB[0]], temp); 
2469                                 
2470                                 if(!((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [collpair->point_indexB[1]].goal >= SOFTGOALSNAP)))
2471                                         VECSUB(cloth->current_x[collpair->point_indexB[1]], cloth->current_x[collpair->point_indexB[1]], temp);
2472                                 
2473                                 if(!((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [collpair->point_indexB[2]].goal >= SOFTGOALSNAP)))
2474                                         VECSUB(cloth->current_x[collpair->point_indexB[2]], cloth->current_x[collpair->point_indexB[2]], temp);
2475                                         
2476                                 collisions = 1;
2477                                 
2478                         }
2479                         
2480                 }
2481                 
2482                 search = collision_list;
2483                 while(search)
2484                 {
2485                         CollisionPair *coll_pair = search->link;
2486                         
2487                         MEM_freeN(coll_pair);
2488                         search = search->next;
2489                 }
2490                 BLI_linklist_free(collision_list,NULL);
2491
2492                 collision_list = NULL;
2493         }
2494         */
2495         // Test on *simple* selfcollisions
2496         collisions = 1;
2497         count = 0;
2498         current_x = cloth->current_x; // needed for openMP
2499 /*
2500 #pragma omp parallel for private(i,j, collisions) shared(current_x)
2501         for(count = 0; count < 6; count++)
2502         {
2503                 collisions = 0;
2504                 
2505                 for(i = 0; i < cloth->numverts; i++)
2506                 {
2507                         for(j = i + 1; j < cloth->numverts; j++)
2508                         {
2509                                 float temp[3];
2510                                 float length = 0;
2511                                 float mindistance = cloth->selftree->epsilon;
2512                                         
2513                                 if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL)
2514                                 {                       
2515                                         if((cloth->verts [i].goal >= SOFTGOALSNAP)
2516                                         && (cloth->verts [j].goal >= SOFTGOALSNAP))
2517                                         {
2518                                                 continue;
2519                                         }
2520                                 }
2521                                         
2522                                         // check for adjacent points
2523                                 if(BLI_edgehash_haskey ( cloth->edgehash, i, j ))
2524                                 {
2525                                         continue;
2526                                 }
2527                                         
2528                                 VECSUB(temp, current_x[i], current_x[j]);
2529                                         
2530                                 length = Normalize(temp);
2531                                         
2532                                 if(length < mindistance)
2533                                 {
2534                                         float correction = mindistance - length;
2535                                                 
2536                                         if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [i].goal >= SOFTGOALSNAP))
2537                                         {
2538                                                 VecMulf(temp, -correction);
2539                                                 VECADD(current_x[j], current_x[j], temp);
2540                                         }
2541                                         else if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [j].goal >= SOFTGOALSNAP))
2542                                         {
2543                                                 VecMulf(temp, correction);
2544                                                 VECADD(current_x[i], current_x[i], temp);
2545                                         }
2546                                         else
2547                                         {
2548                                                 VecMulf(temp, -correction*0.5);
2549                                                 VECADD(current_x[j], current_x[j], temp);
2550                                                 
2551                                                 VECSUB(current_x[i], current_x[i], temp);       
2552                                         }
2553                                         
2554                                         collisions = 1;
2555                                 }
2556                         }
2557                 }
2558         }
2559
2560         
2561         //////////////////////////////////////////////
2562         // SELFCOLLISIONS: update velocities
2563         //////////////////////////////////////////////
2564         for(i = 0; i < cloth->numverts; i++)
2565         {
2566                 VECSUB(cloth->current_v[i], cloth->current_x[i], cloth->current_xold[i]);
2567         }
2568         //////////////////////////////////////////////
2569 */      
2570         return 1;
2571 }