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