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