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