New: pointcache integrated with cloth. Maybe some little glitches left there
[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(float (*fLongVector)[3], 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(float (*fLongVector)[3])
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(float (*fLongVector)[3], 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], float (*fLongVector)[3], 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], float (*fLongVector)[3], 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(float (*fLongVectorA)[3], float (*fLongVectorB)[3], 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], float (*fLongVectorA)[3], float (*fLongVectorB)[3], 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], float (*fLongVectorA)[3], float (*fLongVectorB)[3], 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], float (*fLongVectorA)[3], float aS, float (*fLongVectorB)[3], 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], float (*fLongVectorA)[3], float (*fLongVectorB)[3], 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], float (*fLongVectorA)[3], float (*fLongVectorB)[3], 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, float (*fLongVector)[3])
562 {
563         unsigned int i = 0;
564         zero_lfvector(to, from[0].vcount);
565         /* process diagonal elements */ 
566         for(i = 0; i < from[0].vcount; i++)
567         {
568                 muladd_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);       
569         }
570
571         /* process off-diagonal entries (every off-diagonal entry needs to be symmetric) */
572         // TODO: pragma below is wrong, correct it!
573 // #pragma omp parallel for shared(to,from, fLongVector) private(i) 
574         for(i = from[0].vcount; i < from[0].vcount+from[0].scount; i++)
575         {
576                 unsigned int row = from[i].r;
577                 unsigned int column = from[i].c;
578                 
579                 // muladd_fmatrix_fvector(to[from[i].c], from[i].m, fLongVector[from[i].r]);
580                 
581                 to[column][0] += INPR(from[i].m[0],fLongVector[row]);
582                 to[column][1] += INPR(from[i].m[1],fLongVector[row]);
583                 to[column][2] += INPR(from[i].m[2],fLongVector[row]);   
584         }
585 // #pragma omp parallel for shared(to,from, fLongVector) private(i)     
586         for(i = from[0].vcount; i < from[0].vcount+from[0].scount; i++)
587         {
588                 unsigned int row = from[i].r;
589                 unsigned int column = from[i].c;
590                 
591                 // muladd_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);
592                 
593                 to[row][0] += INPR(from[i].m[0],fLongVector[column]);
594                 to[row][1] += INPR(from[i].m[1],fLongVector[column]);
595                 to[row][2] += INPR(from[i].m[2],fLongVector[column]);   
596         }
597         
598         
599 }
600 /* SPARSE SYMMETRIC add big matrix with big matrix: A = B + C*/
601 DO_INLINE void add_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
602 {
603         unsigned int i = 0;
604
605         /* process diagonal elements */
606         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
607         {
608                 add_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
609         }
610
611 }
612 /* SPARSE SYMMETRIC add big matrix with big matrix: A += B + C */
613 DO_INLINE void addadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
614 {
615         unsigned int i = 0;
616
617         /* process diagonal elements */
618         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
619         {
620                 addadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
621         }
622
623 }
624 /* SPARSE SYMMETRIC subadd big matrix with big matrix: A -= B + C */
625 DO_INLINE void subadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
626 {
627         unsigned int i = 0;
628
629         /* process diagonal elements */
630         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
631         {
632                 subadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
633         }
634
635 }
636 /*  A = B - C (SPARSE SYMMETRIC sub big matrix with big matrix) */
637 DO_INLINE void sub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
638 {
639         unsigned int i = 0;
640
641         /* process diagonal elements */
642         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
643         {
644                 sub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
645         }
646
647 }
648 /* SPARSE SYMMETRIC sub big matrix with big matrix S (special constraint matrix with limited entries) */
649 DO_INLINE void sub_bfmatrix_Smatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
650 {
651         unsigned int i = 0;
652
653         /* process diagonal elements */
654         for(i = 0; i < matrix[0].vcount; i++)
655         {
656                 sub_fmatrix_fmatrix(to[matrix[i].c].m, from[matrix[i].c].m, matrix[i].m);       
657         }
658
659 }
660 /* A += B - C (SPARSE SYMMETRIC addsub big matrix with big matrix) */
661 DO_INLINE void addsub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
662 {
663         unsigned int i = 0;
664
665         /* process diagonal elements */
666         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
667         {
668                 addsub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
669         }
670
671 }
672 /* SPARSE SYMMETRIC sub big matrix with big matrix*/
673 /* A -= B * float + C * float --> for big matrix */
674 /* VERIFIED */
675 DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, float aS,  fmatrix3x3 *matrix, float bS)
676 {
677         unsigned int i = 0;
678
679         /* process diagonal elements */
680         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
681         {
682                 subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS);      
683         }
684
685 }
686
687 ///////////////////////////////////////////////////////////////////
688 // simulator start
689 ///////////////////////////////////////////////////////////////////
690 static float I[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
691 static float ZERO[3][3] = {{0,0,0}, {0,0,0}, {0,0,0}};
692 typedef struct Implicit_Data 
693 {
694         lfVector *X, *V, *Xnew, *Vnew, *olddV, *F, *B, *dV, *z;
695         fmatrix3x3 *A, *dFdV, *dFdX, *S, *P, *Pinv, *bigI; 
696 } Implicit_Data;
697
698 int implicit_init (Object *ob, ClothModifierData *clmd)
699 {
700         unsigned int i = 0;
701         unsigned int pinned = 0;
702         Cloth *cloth = NULL;
703         ClothVertex *verts = NULL;
704         ClothSpring *spring = NULL;
705         Implicit_Data *id = NULL;
706         LinkNode *search = NULL;
707
708         // init memory guard
709         // MEMORY_BASE.first = MEMORY_BASE.last = NULL;
710
711         cloth = (Cloth *)clmd->clothObject;
712         verts = cloth->verts;
713
714         // create implicit base
715         id = (Implicit_Data *)MEM_callocN (sizeof(Implicit_Data), "implicit vecmat");
716         cloth->implicit = id;
717
718         /* process diagonal elements */         
719         id->A = create_bfmatrix(cloth->numverts, cloth->numsprings);
720         id->dFdV = create_bfmatrix(cloth->numverts, cloth->numsprings);
721         id->dFdX = create_bfmatrix(cloth->numverts, cloth->numsprings);
722         id->S = create_bfmatrix(cloth->numverts, 0);
723         id->Pinv = create_bfmatrix(cloth->numverts, cloth->numsprings);
724         id->P = create_bfmatrix(cloth->numverts, cloth->numsprings);
725         id->bigI = create_bfmatrix(cloth->numverts, cloth->numsprings); // TODO 0 springs
726         id->X = create_lfvector(cloth->numverts);
727         id->Xnew = create_lfvector(cloth->numverts);
728         id->V = create_lfvector(cloth->numverts);
729         id->Vnew = create_lfvector(cloth->numverts);
730         id->olddV = create_lfvector(cloth->numverts);
731         zero_lfvector(id->olddV, cloth->numverts);
732         id->F = create_lfvector(cloth->numverts);
733         id->B = create_lfvector(cloth->numverts);
734         id->dV = create_lfvector(cloth->numverts);
735         id->z = create_lfvector(cloth->numverts);
736         
737         for(i=0;i<cloth->numverts;i++) 
738         {
739                 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;
740
741                 if(verts [i].goal >= SOFTGOALSNAP)
742                 {
743                         id->S[pinned].pinned = 1;
744                         id->S[pinned].c = id->S[pinned].r = i;
745                         pinned++;
746                 }
747         }
748
749         // S is special and needs specific vcount and scount
750         id->S[0].vcount = pinned; id->S[0].scount = 0;
751
752         // init springs */
753         search = cloth->springs;
754         for(i=0;i<cloth->numsprings;i++) 
755         {
756                 spring = search->link;
757                 
758                 // dFdV_start[i].r = big_I[i].r = big_zero[i].r = 
759                 id->A[i+cloth->numverts].r = id->dFdV[i+cloth->numverts].r = id->dFdX[i+cloth->numverts].r = 
760                                 id->P[i+cloth->numverts].r = id->Pinv[i+cloth->numverts].r = id->bigI[i+cloth->numverts].r = spring->ij;
761
762                 // dFdV_start[i].c = big_I[i].c = big_zero[i].c = 
763                 id->A[i+cloth->numverts].c = id->dFdV[i+cloth->numverts].c = id->dFdX[i+cloth->numverts].c = 
764                         id->P[i+cloth->numverts].c = id->Pinv[i+cloth->numverts].c = id->bigI[i+cloth->numverts].c = spring->kl;
765
766                 spring->matrix_index = i + cloth->numverts;
767                 
768                 search = search->next;
769         }
770
771         for(i = 0; i < cloth->numverts; i++)
772         {               
773                 VECCOPY(id->X[i], cloth->x[i]);
774         }
775
776         return 1;
777 }
778 int     implicit_free (ClothModifierData *clmd)
779 {
780         Implicit_Data *id;
781         Cloth *cloth;
782         cloth = (Cloth *)clmd->clothObject;
783
784         if(cloth)
785         {
786                 id = cloth->implicit;
787
788                 if(id)
789                 {
790                         del_bfmatrix(id->A);
791                         del_bfmatrix(id->dFdV);
792                         del_bfmatrix(id->dFdX);
793                         del_bfmatrix(id->S);
794                         del_bfmatrix(id->P);
795                         del_bfmatrix(id->Pinv);
796                         del_bfmatrix(id->bigI);
797
798                         del_lfvector(id->X);
799                         del_lfvector(id->Xnew);
800                         del_lfvector(id->V);
801                         del_lfvector(id->Vnew);
802                         del_lfvector(id->olddV);
803                         del_lfvector(id->F);
804                         del_lfvector(id->B);
805                         del_lfvector(id->dV);
806                         del_lfvector(id->z);
807
808                         MEM_freeN(id);
809                 }
810         }
811
812         return 1;
813 }
814
815 void cloth_bending_mode(ClothModifierData *clmd, int enabled)
816 {
817         Cloth *cloth = clmd->clothObject;
818         Implicit_Data *id;
819         
820         if(cloth)
821         {
822                 id = cloth->implicit;
823                 
824                 if(id)
825                 {
826                         if(enabled)
827                         {
828                                 cloth->numsprings = cloth->numspringssave;
829                         }
830                         else
831                         {
832                                 cloth->numsprings = cloth->numothersprings;
833                         }
834                         
835                         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;
836                 }       
837         }
838 }
839
840 DO_INLINE float fb(float length, float L)
841 {
842         float x = length/L;
843         return (-11.541f*pow(x,4)+34.193f*pow(x,3)-39.083f*pow(x,2)+23.116f*x-9.713f);
844 }
845
846 DO_INLINE float fbderiv(float length, float L)
847 {
848         float x = length/L;
849
850         return (-46.164f*pow(x,3)+102.579f*pow(x,2)-78.166f*x+23.116f);
851 }
852
853 DO_INLINE float fbstar(float length, float L, float kb, float cb)
854 {
855         float tempfb = kb * fb(length, L);
856
857         float fbstar = cb * (length - L);
858
859         if(tempfb < fbstar)
860                 return fbstar;
861         else
862                 return tempfb;          
863 }
864
865 DO_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
866 {
867         float tempfb = kb * fb(length, L);
868         float fbstar = cb * (length - L);
869
870         if(tempfb < fbstar)
871         {               
872                 return cb;
873         }
874         else
875         {
876                 return kb * fbderiv(length, L); 
877         }       
878 }
879
880 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
881 {
882         unsigned int i=0;
883
884         for(i=0;i<S[0].vcount;i++)
885         {
886                 mul_fvector_fmatrix(V[S[i].r], V[S[i].r], S[i].m);
887         }
888 }
889
890 // block diagonalizer
891 void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *S, fmatrix3x3 *bigI)
892 {
893         unsigned int i=0;
894
895         // Take only the diagonal blocks of A
896         for(i=0;i<lA[0].vcount;i++)
897         {
898                 cp_fmatrix(P[i].m, lA[i].m); 
899         }
900         /*
901         // SpecialBigSMul(P, S, P);
902         for(i=0;i<S[0].vcount;i++)
903         {
904         mul_fmatrix_fmatrix(P[S[i].r].m, S[i].m, P[S[i].r].m);
905         }
906         add_bfmatrix_bfmatrix(P, P, bigI);
907         */
908         for(i=0;i<lA[0].vcount;i++)                              
909         {
910                 inverse_fmatrix(Pinv[i].m, P[i].m); 
911         }               
912
913 }
914
915 int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S)
916 {
917         // Solves for unknown X in equation AX=B
918         unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
919         float conjgrad_epsilon=0.0001f, conjgrad_lasterror=0;
920         lfVector *q, *d, *tmp, *r; 
921         float s, starget, a, s_prev;
922         unsigned int numverts = lA[0].vcount;
923         q = create_lfvector(numverts);
924         d = create_lfvector(numverts);
925         tmp = create_lfvector(numverts);
926         r = create_lfvector(numverts);
927
928         // zero_lfvector(ldV, CLOTHPARTICLES);
929         filter(ldV, S);
930
931         add_lfvector_lfvector(ldV, ldV, z, numverts);
932
933         // r = B - Mul(tmp,A,X);    // just use B if X known to be zero
934         cp_lfvector(r, lB, numverts);
935         mul_bfmatrix_lfvector(tmp, lA, ldV);
936         sub_lfvector_lfvector(r, r, tmp, numverts);
937
938         filter(r,S);
939
940         cp_lfvector(d, r, numverts);
941
942         s = dot_lfvector(r, r, numverts);
943         starget = s * sqrt(conjgrad_epsilon);
944
945         while((s>starget && conjgrad_loopcount < conjgrad_looplimit))
946         {       
947                 // Mul(q,A,d); // q = A*d;
948                 mul_bfmatrix_lfvector(q, lA, d);
949
950                 filter(q,S);
951
952                 a = s/dot_lfvector(d, q, numverts);
953
954                 // X = X + d*a;
955                 add_lfvector_lfvectorS(ldV, ldV, d, a, numverts);
956
957                 // r = r - q*a;
958                 sub_lfvector_lfvectorS(r, r, q, a, numverts);
959
960                 s_prev = s;
961                 s = dot_lfvector(r, r, numverts);
962
963                 //d = r+d*(s/s_prev);
964                 add_lfvector_lfvectorS(d, r, d, (s/s_prev), numverts);
965
966                 filter(d,S);
967
968                 conjgrad_loopcount++;
969         }
970         conjgrad_lasterror = s;
971
972         del_lfvector(q);
973         del_lfvector(d);
974         del_lfvector(tmp);
975         del_lfvector(r);
976         // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount);
977
978         return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
979 }
980 /*
981 int cg_filtered_pre(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, lfVector *X0, fmatrix3x3 *P, fmatrix3x3 *Pinv, float dt)
982 {
983 // Solves for unknown X in equation AX=B
984 unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
985 float conjgrad_epsilon=0.0001f, conjgrad_lasterror=0;
986 lfVector *q, *c , *tmp, *r, *s, *filterX0, *p_fb, *bhat;
987 float delta0, deltanew, deltaold, alpha=0, epsilon_sqr;
988 unsigned int numverts = lA[0].vcount;
989 int i = 0;
990 q = create_lfvector(numverts);
991 c = create_lfvector(numverts);
992 tmp = create_lfvector(numverts);
993 r = create_lfvector(numverts);
994 s = create_lfvector(numverts);
995 filterX0 = create_lfvector(numverts);
996 p_fb = create_lfvector(numverts);
997 bhat = create_lfvector(numverts);
998
999 // SpecialBigSSub(bigI, S);
1000 initdiag_bfmatrix(bigI, I);
1001 sub_bfmatrix_Smatrix(bigI, bigI, S); // TODO
1002
1003 BuildPPinv(lA,P,Pinv,S, bigI);
1004
1005 //////////////////////////
1006 // x = S*x0 + (I-S)*z 
1007 //////////////////////////
1008 // filterX0 = X0 * 1.0f;
1009 cp_lfvector(filterX0, X0, numverts);
1010 // filter(filterX0,S);
1011 filter(filterX0, S);
1012 // X = filterX0 * 1.0f;
1013 cp_lfvector(ldV, filterX0, numverts);
1014
1015 // X = X + Mul(tmp, bigI, z);
1016 mul_bfmatrix_lfvector(tmp, bigI, z);
1017 add_lfvector_lfvector(ldV, ldV, tmp, numverts);
1018 //////////////////////////
1019
1020
1021 //////////////////////////
1022 // b_hat = S*(b-A*(I-S)*z) 
1023 //////////////////////////      
1024 // bhat = bigI * z;
1025 mul_bfmatrix_lfvector(bhat, bigI, z);
1026 // bhat = Mul(tmp, A, bhat);
1027 mul_bfmatrix_lfvector(tmp, lA, bhat);
1028 cp_lfvector(bhat, tmp, numverts);
1029 // bhat = B - bhat;
1030 sub_lfvector_lfvector(bhat, lB, bhat, numverts);
1031 // cp_lfvector(bhat, lB, numverts);
1032 filter(bhat,S);
1033 //////////////////////////
1034
1035 //////////////////////////
1036 // r = S*(b - A*x)  
1037 //////////////////////////
1038 // r = B - Mul(tmp,A,X);    // just use B if X known to be zero
1039 mul_bfmatrix_lfvector(tmp, lA, ldV);
1040 sub_lfvector_lfvector(r, lB, tmp, numverts);
1041 // cp_lfvector(r, lB, numverts);
1042 filter(r,S);
1043 //////////////////////////
1044
1045
1046 //////////////////////////
1047 // (p) = c = S * P^-1 * r
1048 //////////////////////////
1049 // c = Pinv * r;
1050 mul_bfmatrix_lfvector(c, Pinv, r);
1051 filter(c,S);
1052 //////////////////////////      
1053
1054
1055 //////////////////////////
1056 // p_fb = P * bhat
1057 // delta0 = dot(bhat, p_fb)
1058 //////////////////////////
1059 // p_fb = P*bhat;       
1060 mul_bfmatrix_lfvector(p_fb, P, bhat);
1061 delta0 = dot_lfvector(bhat, p_fb, numverts);
1062 //////////////////////////
1063
1064
1065 //////////////////////////
1066 // deltanew = dot(r,c)
1067 //////////////////////////
1068 deltanew = dot_lfvector(r, c, numverts);
1069 //////////////////////////
1070 epsilon_sqr = conjgrad_epsilon*conjgrad_epsilon; // paper mentiones dt * 0.01
1071
1072 while((deltanew>(epsilon_sqr*delta0))&& (conjgrad_loopcount++ < conjgrad_looplimit))
1073 {
1074 //////////////////////////
1075 // (s) = q = S*A*c
1076 //////////////////////////
1077 // q = A*c; 
1078 mul_bfmatrix_lfvector(q, lA, c);
1079 filter(q,S);
1080 //////////////////////////              
1081
1082 //////////////////////////
1083 // alpha = deltanew / (c^T * q)
1084 //////////////////////////
1085 alpha = deltanew/dot_lfvector(c, q, numverts);
1086 //////////////////////////              
1087
1088 //X = X + c*alpha;
1089 add_lfvector_lfvectorS(ldV, ldV, c, alpha, numverts);
1090 //r = r - q*alpha;
1091 sub_lfvector_lfvectorS(r, r, q, alpha, numverts);               
1092
1093 //////////////////////////
1094 // (h) = s = P^-1 * r
1095 //////////////////////////
1096 // s = Pinv * r;
1097 mul_bfmatrix_lfvector(s, Pinv, r);
1098 filter(s,S);
1099 //////////////////////////
1100
1101 deltaold = deltanew;
1102
1103 // deltanew = dot(r,s);
1104 deltanew = dot_lfvector(r, s, numverts);
1105
1106 //////////////////////////
1107 // c = S * (s + (deltanew/deltaold)*c)
1108 //////////////////////////      
1109 // c = s + c * (deltanew/deltaold);
1110 add_lfvector_lfvectorS(c, s, c, (deltanew/deltaold), numverts);
1111 filter(c,S);
1112 //////////////////////////
1113
1114 }
1115 conjgrad_lasterror = deltanew;
1116 del_lfvector(q);
1117 del_lfvector(c);
1118 del_lfvector(tmp);
1119 del_lfvector(r);
1120 del_lfvector(s);
1121 del_lfvector(filterX0);
1122 del_lfvector(p_fb);
1123 del_lfvector(bhat);
1124 printf("Bconjgrad_loopcount: %d\n", conjgrad_loopcount);
1125
1126 return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
1127 }
1128 */
1129
1130 // outer product is NOT cross product!!!
1131 DO_INLINE void dfdx_spring_type1(float to[3][3], float dir[3],float length,float L,float k)
1132 {
1133         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1134         // return  (outerprod(dir,dir)*k + (I - outerprod(dir,dir))*(k - ((k*L)/length)));
1135         float temp[3][3];
1136         mul_fvectorT_fvector(temp, dir, dir);
1137         sub_fmatrix_fmatrix(to, I, temp);
1138         mul_fmatrix_S(to, k* (1.0f-(L/length)));
1139         mul_fmatrix_S(temp, k);
1140         add_fmatrix_fmatrix(to, temp, to);
1141 }
1142
1143 DO_INLINE void dfdx_spring_type2(float to[3][3], float dir[3],float length,float L,float k, float cb)
1144 {
1145         // return  outerprod(dir,dir)*fbstar_jacobi(length, L, k, cb);
1146         mul_fvectorT_fvectorS(to, dir, dir, fbstar_jacobi(length, L, k, cb));
1147 }
1148
1149 DO_INLINE void dfdv_damp(float to[3][3], float dir[3], float damping)
1150 {
1151         // derivative of force wrt velocity.  
1152         // return outerprod(dir,dir) * damping;
1153         mul_fvectorT_fvectorS(to, dir, dir, damping);
1154 }
1155
1156 DO_INLINE void dfdx_spring(float to[3][3],  float dir[3],float length,float L,float k)
1157 {
1158         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1159         //return  ( (I-outerprod(dir,dir))*Min(1.0f,rest/length) - I) * -k;
1160         mul_fvectorT_fvector(to, dir, dir);
1161         sub_fmatrix_fmatrix(to, I, to);
1162         mul_fmatrix_S(to, (((L/length)> 1.0f) ? (1.0f): (L/length))); 
1163         sub_fmatrix_fmatrix(to, to, I);
1164         mul_fmatrix_S(to, -k);
1165 }
1166
1167 DO_INLINE void dfdx_damp(float to[3][3],  float dir[3],float length,const float vel[3],float rest,float damping)
1168 {
1169         // inner spring damping   vel is the relative velocity  of the endpoints.  
1170         //      return (I-outerprod(dir,dir)) * (-damping * -(dot(dir,vel)/Max(length,rest)));
1171         mul_fvectorT_fvector(to, dir, dir);
1172         sub_fmatrix_fmatrix(to, I, to);
1173         mul_fmatrix_S(to,  (-damping * -(INPR(dir,vel)/MAX2(length,rest)))); 
1174
1175 }
1176
1177 DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1178 {
1179         float extent[3];
1180         float length = 0;
1181         float dir[3] = {0,0,0};
1182         float vel[3];
1183         float k = 0.0f;
1184         float L = s->restlen;
1185         float cb = clmd->sim_parms.structural;
1186
1187         float nullf[3] = {0,0,0};
1188         float stretch_force[3] = {0,0,0};
1189         float bending_force[3] = {0,0,0};
1190         float damping_force[3] = {0,0,0};
1191         float nulldfdx[3][3]={ {0,0,0}, {0,0,0}, {0,0,0}};
1192         
1193         VECCOPY(s->f, nullf);
1194         cp_fmatrix(s->dfdx, nulldfdx);
1195         cp_fmatrix(s->dfdv, nulldfdx);
1196
1197         // calculate elonglation
1198         VECSUB(extent, X[s->kl], X[s->ij]);
1199         VECSUB(vel, V[s->kl], V[s->ij]);
1200         length = sqrt(INPR(extent, extent));
1201         
1202         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
1203         
1204         if(length > ABS(ALMOST_ZERO))
1205         {
1206                 /*
1207                 if(length>L)
1208                 {
1209                         if((clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED) 
1210                         && ((((length-L)*100.0f/L) > clmd->sim_parms.maxspringlen))) // cut spring!
1211                         {
1212                                 s->flags |= CSPRING_FLAG_DEACTIVATE;
1213                                 return;
1214                         }
1215                 } 
1216                 */
1217                 mul_fvector_S(dir, extent, 1.0f/length);
1218         }
1219         else    
1220         {
1221                 mul_fvector_S(dir, extent, 0.0f);
1222         }
1223         
1224         
1225         // calculate force of structural + shear springs
1226         if(s->type != CLOTH_SPRING_TYPE_BENDING)
1227         {
1228                 if(length > L) // only on elonglation
1229                 {
1230                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1231
1232                         k = clmd->sim_parms.structural; 
1233
1234                         mul_fvector_S(stretch_force, dir, (k*(length-L))); 
1235
1236                         VECADD(s->f, s->f, stretch_force);
1237
1238                         // Ascher & Boxman, p.21: Damping only during elonglation
1239                         mul_fvector_S(damping_force, extent, clmd->sim_parms.Cdis * ((INPR(vel,extent)/length))); 
1240                         VECADD(s->f, s->f, damping_force);
1241
1242                         dfdx_spring_type1(s->dfdx, dir,length,L,k);
1243
1244                         dfdv_damp(s->dfdv, dir,clmd->sim_parms.Cdis);
1245                 }
1246         }
1247         else // calculate force of bending springs
1248         {
1249                 if(length < L)
1250                 {
1251                         // clmd->sim_parms.flags |= CLOTH_SIMSETTINGS_FLAG_BIG_FORCE;
1252                         
1253                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1254                         
1255                         k = clmd->sim_parms.bending;    
1256
1257                         mul_fvector_S(bending_force, dir, fbstar(length, L, k, cb));
1258                         VECADD(s->f, s->f, bending_force);
1259                         
1260                         if(INPR(bending_force,bending_force) > 0.13*0.13)
1261                         {
1262                                 clmd->sim_parms.flags |= CLOTH_SIMSETTINGS_FLAG_BIG_FORCE;
1263                         }
1264                         
1265                         dfdx_spring_type2(s->dfdx, dir,length,L,k, cb);
1266                 }
1267         }
1268 }
1269
1270 DO_INLINE int cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1271 {
1272         if(s->flags & CLOTH_SPRING_FLAG_NEEDED)
1273         {
1274                 VECADD(lF[s->ij], lF[s->ij], s->f);
1275                 VECSUB(lF[s->kl], lF[s->kl], s->f);     
1276                         
1277                 if(s->type != CLOTH_SPRING_TYPE_BENDING)
1278                 {
1279                         sub_fmatrix_fmatrix(dFdV[s->ij].m, dFdV[s->ij].m, s->dfdv);
1280                         sub_fmatrix_fmatrix(dFdV[s->kl].m, dFdV[s->kl].m, s->dfdv);
1281                         add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv); 
1282                 }
1283                 else if(!(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_BIG_FORCE))
1284                         return 0;
1285                 
1286                 sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx);
1287                 sub_fmatrix_fmatrix(dFdX[s->kl].m, dFdX[s->kl].m, s->dfdx);
1288
1289                 add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx);
1290         }
1291         
1292         return 1;
1293 }
1294
1295 DO_INLINE void calculateTriangleNormal(float to[3], lfVector *X, MFace mface)
1296 {
1297         float v1[3], v2[3];
1298
1299         VECSUB(v1, X[mface.v2], X[mface.v1]);
1300         VECSUB(v2, X[mface.v3], X[mface.v1]);
1301         cross_fvector(to, v1, v2);
1302 }
1303
1304 DO_INLINE void calculatQuadNormal(float to[3], lfVector *X, MFace mface)
1305 {
1306         float temp = CalcNormFloat4(X[mface.v1],X[mface.v2],X[mface.v3],X[mface.v4],to);
1307         mul_fvector_S(to, to, temp);
1308 }
1309
1310 void calculateWeightedVertexNormal(ClothModifierData *clmd, MFace *mfaces, float to[3], int index, lfVector *X)
1311 {
1312         float temp[3]; 
1313         int i;
1314         Cloth *cloth = clmd->clothObject;
1315
1316         for(i = 0; i < cloth->numfaces; i++)
1317         {
1318                 // check if this triangle contains the selected vertex
1319                 if(mfaces[i].v1 == index || mfaces[i].v2 == index || mfaces[i].v3 == index || mfaces[i].v4 == index)
1320                 {
1321                         calculatQuadNormal(temp, X, mfaces[i]);
1322                         VECADD(to, to, temp);
1323                 }
1324         }
1325 }
1326 float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
1327 {
1328         return fabs(INPR(wind, vertexnormal) * 0.5f);
1329 }
1330
1331 DO_INLINE void calc_triangle_force(ClothModifierData *clmd, MFace mface, lfVector *F, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors)
1332 {       
1333
1334 }
1335
1336 void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVector *lV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors, float time)
1337 {
1338         /* Collect forces and derivatives:  F,dFdX,dFdV */
1339         Cloth           *cloth          = clmd->clothObject;
1340         unsigned int    i               = 0;
1341         float           spring_air      = clmd->sim_parms.Cvi * 0.01f; /* viscosity of air scaled in percent */
1342         float           gravity[3];
1343         float           tm2[3][3]       = {{-spring_air,0,0}, {0,-spring_air,0},{0,0,-spring_air}};
1344         ClothVertex *verts = cloth->verts;
1345         MFace           *mfaces         = cloth->mfaces;
1346         float wind_normalized[3];
1347         unsigned int numverts = cloth->numverts;
1348         float auxvect[3], velgoal[3], tvect[3];
1349         float kd, ks;
1350         LinkNode *search = cloth->springs;
1351
1352         VECCOPY(gravity, clmd->sim_parms.gravity);
1353         mul_fvector_S(gravity, gravity, 0.001f); /* scale gravity force */
1354
1355         /* set dFdX jacobi matrix to zero */
1356         init_bfmatrix(dFdX, ZERO);
1357         /* set dFdX jacobi matrix diagonal entries to -spring_air */ 
1358         initdiag_bfmatrix(dFdV, tm2);
1359
1360         init_lfvector(lF, gravity, numverts);
1361
1362         submul_lfvectorS(lF, lV, spring_air, numverts);
1363
1364         /* do goal stuff */
1365         if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
1366         {       
1367                 for(i = 0; i < numverts; i++)
1368                 {                       
1369                         if(verts [i].goal < SOFTGOALSNAP)
1370                         {                       
1371                                 // current_position = xold + t * (newposition - xold)
1372                                 VECSUB(tvect, cloth->xconst[i], cloth->xold[i]);
1373                                 mul_fvector_S(tvect, tvect, time);
1374                                 VECADD(tvect, tvect, cloth->xold[i]);
1375
1376                                 VECSUB(auxvect, tvect, lX[i]);
1377                                 ks  = 1.0f/(1.0f- verts [i].goal*clmd->sim_parms.goalspring)-1.0f ;
1378                                 VECADDS(lF[i], lF[i], auxvect, -ks);
1379
1380                                 // calulate damping forces generated by goals                           
1381                                 VECSUB(velgoal, cloth->xold[i], cloth->xconst[i]);
1382                                 kd =  clmd->sim_parms.goalfrict * 0.01f; // friction force scale taken from SB
1383                                 VECSUBADDSS(lF[i], velgoal, kd, lV[i], kd);
1384                                 
1385                         }
1386                 }       
1387         }
1388
1389         /* handle external forces like wind */
1390         if(effectors)
1391         {
1392                 float speed[3] = {0.0f, 0.0f,0.0f};
1393                 float force[3]= {0.0f, 0.0f, 0.0f};
1394                 
1395                 #pragma omp parallel for private (i) shared(lF)
1396                 for(i = 0; i < cloth->numverts; i++)
1397                 {
1398                         float vertexnormal[3]={0,0,0};
1399                         float fieldfactor = 1000.0f, windfactor  = 250.0f; // from sb
1400                         
1401                         pdDoEffectors(effectors, lX[i], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);          
1402                         
1403                         // TODO apply forcefields here
1404                         VECADDS(lF[i], lF[i], force, fieldfactor*0.01f);
1405
1406                         VECCOPY(wind_normalized, speed);
1407                         Normalize(wind_normalized);
1408                         
1409                         calculateWeightedVertexNormal(clmd, mfaces, vertexnormal, i, lX);
1410                         VECADDS(lF[i], lF[i], wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
1411                 }
1412         }
1413         
1414         // calculate spring forces
1415         search = cloth->springs;
1416         while(search)
1417         {
1418                 // only handle active springs
1419                 // if(((clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED)){}
1420                 cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX);
1421
1422                 search = search->next;
1423         }
1424         
1425         if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_BIG_FORCE)
1426         {       
1427                 if(cloth->numspringssave != cloth->numsprings)
1428                 {
1429                         cloth_bending_mode(clmd, 1);
1430                 }
1431         }
1432         else
1433         {
1434                 if(cloth->numspringssave == cloth->numsprings)
1435                 {
1436                         cloth_bending_mode(clmd, 0);
1437                 }
1438         }
1439         
1440         // apply spring forces
1441         search = cloth->springs;
1442         while(search)
1443         {
1444                 // only handle active springs
1445                 // if(((clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED))    
1446                 if(!cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX))
1447                         break;
1448                 search = search->next;
1449         }
1450         
1451         clmd->sim_parms.flags &= ~CLOTH_SIMSETTINGS_FLAG_BIG_FORCE;
1452         
1453 }
1454
1455 void simulate_implicit_euler(lfVector *Vnew, lfVector *lX, lfVector *lV, lfVector *lF, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float dt, fmatrix3x3 *A, lfVector *B, lfVector *dV, fmatrix3x3 *S, lfVector *z, lfVector *olddV, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1456 {
1457         unsigned int numverts = dFdV[0].vcount;
1458
1459         lfVector *dFdXmV = create_lfvector(numverts);
1460         initdiag_bfmatrix(A, I);
1461         zero_lfvector(dV, numverts);
1462
1463         subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt));
1464
1465         mul_bfmatrix_lfvector(dFdXmV, dFdX, lV);
1466
1467         add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, (dt*dt), numverts);
1468         
1469         itstart();
1470         
1471         cg_filtered(dV, A, B, z, S); /* conjugate gradient algorithm to solve Ax=b */
1472         // cg_filtered_pre(dV, A, B, z, olddV, P, Pinv, dt);
1473         
1474         itend();
1475         // printf("cg_filtered calc time: %f\n", (float)itval());
1476         
1477         cp_lfvector(olddV, dV, numverts);
1478
1479         // advance velocities
1480         add_lfvector_lfvector(Vnew, lV, dV, numverts);
1481         
1482
1483         del_lfvector(dFdXmV);
1484 }
1485
1486 int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
1487 {               
1488         unsigned int i=0;
1489         float step=0.0f, tf=1.0f;
1490         Cloth *cloth = clmd->clothObject;
1491         ClothVertex *verts = cloth->verts;
1492         unsigned int numverts = cloth->numverts;
1493         float dt = 1.0f / clmd->sim_parms.stepsPerFrame;
1494         Implicit_Data *id = cloth->implicit;
1495         int result = 0;
1496         float force = 0, lastforce = 0;
1497         lfVector *dx;
1498         
1499         if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1500         {
1501                 for(i = 0; i < numverts; i++)
1502                 {                       
1503                         // update velocities with constrained velocities from pinned verts
1504                         if(verts [i].goal >= SOFTGOALSNAP)
1505                         {                       
1506                                 VECSUB(id->V[i], cloth->xconst[i], cloth->xold[i]);
1507                                 // VecMulf(id->V[i], 1.0 / dt);
1508                         }
1509                 }       
1510         }
1511
1512         while(step < tf)
1513         {               
1514                 effectors= pdInitEffectors(ob,NULL);
1515                 
1516                 // calculate 
1517                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step );
1518                 
1519                 // check for sleeping
1520                 if(!(clmd->coll_parms.flags & CLOTH_SIMSETTINGS_FLAG_SLEEP))
1521                 {
1522                         simulate_implicit_euler(id->Vnew, id->X, id->V, id->F, id->dFdV, id->dFdX, dt, id->A, id->B, id->dV, id->S, id->z, id->olddV, id->P, id->Pinv);
1523                 
1524                         add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
1525                 }
1526                 
1527                 dx = create_lfvector(numverts);
1528                 sub_lfvector_lfvector(dx, id->Xnew, id->X, numverts);
1529                 force = dot_lfvector(dx, dx, numverts);
1530                 del_lfvector(dx);
1531                 
1532                 if((force < 0.00001) && (lastforce >= force))
1533                         clmd->coll_parms.flags |= CLOTH_SIMSETTINGS_FLAG_SLEEP;
1534                 else if((lastforce*2 < force))
1535                         clmd->coll_parms.flags &= ~CLOTH_SIMSETTINGS_FLAG_SLEEP;
1536                 
1537                 lastforce = force;
1538                 
1539                 if(clmd->coll_parms.flags & CLOTH_COLLISIONSETTINGS_FLAG_ENABLED)
1540                 {
1541                         // collisions 
1542                         // itstart();
1543                         
1544                         // update verts to current positions
1545                         for(i = 0; i < numverts; i++)
1546                         {               
1547                                 if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1548                                 {                       
1549                                         if(verts [i].goal >= SOFTGOALSNAP)
1550                                         {                       
1551                                                 float tvect[3] = {.0,.0,.0};
1552                                                 // VECSUB(tvect, id->Xnew[i], verts[i].xold);
1553                                                 mul_fvector_S(tvect, id->V[i], step+dt);
1554                                                 VECADD(tvect, tvect, cloth->xold[i]);
1555                                                 VECCOPY(id->Xnew[i], tvect);
1556                                         }
1557                                                 
1558                                 }
1559                                 
1560                                 VECCOPY(cloth->current_x[i], id->Xnew[i]);
1561                                 VECSUB(cloth->current_v[i], cloth->current_x[i], cloth->current_xold[i]);
1562                                 VECCOPY(cloth->v[i], cloth->current_v[i]);
1563                         }
1564                         
1565                         // call collision function
1566                         result = cloth_bvh_objcollision(clmd, step + dt, step, dt);
1567                         
1568                         // copy corrected positions back to simulation
1569                         if(result)
1570                         {
1571                                 memcpy(cloth->current_xold, cloth->current_x, sizeof(lfVector) * numverts);
1572                                 memcpy(id->Xnew, cloth->current_x, sizeof(lfVector) * numverts);
1573                                 
1574                                 for(i = 0; i < numverts; i++)
1575                                 {       
1576                                         VECCOPY(id->Vnew[i], cloth->current_v[i]);
1577                                         VecMulf(id->Vnew[i], 1.0f / dt);
1578                                 }
1579                         }
1580                         else
1581                         {
1582                                 memcpy(cloth->current_xold, id->Xnew, sizeof(lfVector) * numverts);
1583                         }
1584                         
1585                         // X = Xnew;
1586                         cp_lfvector(id->X, id->Xnew, numverts);
1587                         
1588                         // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
1589                         if(result)
1590                         {
1591                                 // V = Vnew;
1592                                 cp_lfvector(id->V, id->Vnew, numverts);
1593                                 
1594                                 // calculate 
1595                                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step);       
1596                                 simulate_implicit_euler(id->Vnew, id->X, id->V, id->F, id->dFdV, id->dFdX, dt / 2.0f, id->A, id->B, id->dV, id->S, id->z, id->olddV, id->P, id->Pinv);
1597                         }
1598                         
1599                 }
1600                 else
1601                 {
1602                         // X = Xnew;
1603                         cp_lfvector(id->X, id->Xnew, numverts);
1604                 }
1605                 
1606                 // itend();
1607                 // printf("collision time: %f\n", (float)itval());
1608                 
1609                 // V = Vnew;
1610                 cp_lfvector(id->V, id->Vnew, numverts);
1611                 
1612                 step += dt;
1613
1614                 if(effectors) pdEndEffectors(effectors);
1615         }
1616         
1617         if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL)
1618         {
1619                 for(i = 0; i < numverts; i++)
1620                 {
1621                         if(verts [i].goal < SOFTGOALSNAP)
1622                         {
1623                                 VECCOPY(cloth->current_xold[i], id->X[i]);
1624                                 VECCOPY(cloth->x[i], id->X[i]);
1625                         }
1626                         else
1627                         {
1628                                 VECCOPY(cloth->current_xold[i], cloth->xconst[i]);
1629                                 VECCOPY(cloth->x[i], cloth->xconst[i]);
1630                         }
1631                 }
1632         }
1633         else
1634         {
1635                 memcpy(cloth->current_xold, id->X, sizeof(lfVector) * numverts);
1636                 memcpy(cloth->x, id->X, sizeof(lfVector) * numverts);
1637         }
1638         
1639         memcpy(cloth->v, id->V, sizeof(lfVector) * numverts);
1640         
1641         return 1;
1642 }
1643
1644 void implicit_set_positions (ClothModifierData *clmd)
1645 {               
1646         Cloth *cloth = clmd->clothObject;
1647         unsigned int numverts = cloth->numverts;
1648         Implicit_Data *id = cloth->implicit;
1649         
1650         memcpy(id->X, cloth->x, sizeof(lfVector) * numverts);   
1651         memcpy(id->V, cloth->v, sizeof(lfVector) * numverts);   
1652 }
1653
1654
1655 int collisions_collision_response_static(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1656 {
1657         /*
1658         unsigned int i = 0;
1659         int result = 0;
1660         LinkNode *search = NULL;
1661         CollPair *collpair = NULL;
1662         Cloth *cloth1, *cloth2;
1663         float w1, w2, w3, u1, u2, u3;
1664         float v1[3], v2[3], relativeVelocity[3];
1665         float magrelVel;
1666         
1667         cloth1 = clmd->clothObject;
1668         cloth2 = coll_clmd->clothObject;
1669
1670         // search = clmd->coll_parms.collision_list;
1671         
1672         while(search)
1673         {
1674         collpair = search->link;
1675                 
1676                 // compute barycentric coordinates for both collision points
1677         collisions_compute_barycentric(collpair->pa,
1678         cloth1->verts[collpair->ap1].txold,
1679         cloth1->verts[collpair->ap2].txold,
1680         cloth1->verts[collpair->ap3].txold, 
1681         &w1, &w2, &w3);
1682         
1683         collisions_compute_barycentric(collpair->pb,
1684         cloth2->verts[collpair->bp1].txold,
1685         cloth2->verts[collpair->bp2].txold,
1686         cloth2->verts[collpair->bp3].txold,
1687         &u1, &u2, &u3);
1688         
1689                 // Calculate relative "velocity".
1690         interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
1691                 
1692         interpolateOnTriangle(v2, cloth2->verts[collpair->bp1].tv, cloth2->verts[collpair->bp2].tv, cloth2->verts[collpair->bp3].tv, u1, u2, u3);
1693                 
1694         VECSUB(relativeVelocity, v1, v2);
1695                         
1696                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
1697         magrelVel = INPR(relativeVelocity, collpair->normal);
1698                 
1699                 // printf("magrelVel: %f\n", magrelVel);
1700                                 
1701                 // Calculate masses of points.
1702                 
1703                 // If v_n_mag < 0 the edges are approaching each other.
1704         if(magrelVel < -ALMOST_ZERO) 
1705         {
1706                         // Calculate Impulse magnitude to stop all motion in normal direction.
1707                         // const double I_mag = v_n_mag / (1/m1 + 1/m2);
1708         float magnitude_i = magrelVel / 2.0f; // TODO implement masses
1709         float tangential[3], magtangent, magnormal, collvel[3];
1710         float vrel_t_pre[3];
1711         float vrel_t[3];
1712         double impulse;
1713         float epsilon = clmd->coll_parms.epsilon;
1714         float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
1715                         
1716                         // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms.friction*0.01, magrelVel);
1717                         
1718                         // magtangent = INPR(tangential, tangential);
1719                         
1720                         // Apply friction impulse.
1721         if (magtangent < -ALMOST_ZERO) 
1722         {
1723                                 
1724                                 // printf("friction applied: %f\n", magtangent);
1725                                 // TODO check original code 
1726 }
1727                         
1728
1729         impulse = -2.0f * magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
1730                         
1731                         // printf("impulse: %f\n", impulse);
1732                         
1733                         // face A
1734         VECADDMUL(cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse); 
1735         cloth1->verts[collpair->ap1].impulse_count++;
1736                         
1737         VECADDMUL(cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse); 
1738         cloth1->verts[collpair->ap2].impulse_count++;
1739                         
1740         VECADDMUL(cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse); 
1741         cloth1->verts[collpair->ap3].impulse_count++;
1742                         
1743                         // face B
1744         VECADDMUL(cloth2->verts[collpair->bp1].impulse, collpair->normal, u1 * impulse); 
1745         cloth2->verts[collpair->bp1].impulse_count++;
1746                         
1747         VECADDMUL(cloth2->verts[collpair->bp2].impulse, collpair->normal, u2 * impulse); 
1748         cloth2->verts[collpair->bp2].impulse_count++;
1749                         
1750         VECADDMUL(cloth2->verts[collpair->bp3].impulse, collpair->normal, u3 * impulse); 
1751         cloth2->verts[collpair->bp3].impulse_count++;
1752                         
1753                         
1754         result = 1;     
1755                 
1756                         // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
1757                         
1758                         // Apply the impulse and increase impulse counters.
1759         
1760                         
1761 }
1762                 
1763         search = search->next;
1764 }
1765         
1766         
1767         return result;
1768         */
1769         return 0;
1770 }
1771
1772
1773 int collisions_collision_response_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1774 {
1775         return 0;
1776 }
1777
1778
1779 int collisions_collision_response_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1780 {
1781         return 0;
1782 }
1783
1784 void cloth_collision_static(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
1785 {
1786         /*
1787         CollPair *collpair = NULL;
1788         Cloth *cloth1=NULL, *cloth2=NULL;
1789         MFace *face1=NULL, *face2=NULL;
1790         ClothVertex *verts1=NULL, *verts2=NULL;
1791         double distance = 0;
1792         float epsilon = clmd->coll_parms.epsilon;
1793         unsigned int i = 0;
1794
1795         for(i = 0; i < 4; i++)
1796         {
1797         collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");                
1798                 
1799         cloth1 = clmd->clothObject;
1800         cloth2 = coll_clmd->clothObject;
1801                 
1802         verts1 = cloth1->verts;
1803         verts2 = cloth2->verts;
1804         
1805         face1 = &(cloth1->mfaces[tree1->tri_index]);
1806         face2 = &(cloth2->mfaces[tree2->tri_index]);
1807                 
1808                 // check all possible pairs of triangles
1809         if(i == 0)
1810         {
1811         collpair->ap1 = face1->v1;
1812         collpair->ap2 = face1->v2;
1813         collpair->ap3 = face1->v3;
1814                         
1815         collpair->bp1 = face2->v1;
1816         collpair->bp2 = face2->v2;
1817         collpair->bp3 = face2->v3;
1818                         
1819 }
1820                 
1821         if(i == 1)
1822         {
1823         if(face1->v4)
1824         {
1825         collpair->ap1 = face1->v3;
1826         collpair->ap2 = face1->v4;
1827         collpair->ap3 = face1->v1;
1828                                 
1829         collpair->bp1 = face2->v1;
1830         collpair->bp2 = face2->v2;
1831         collpair->bp3 = face2->v3;
1832 }
1833         else
1834         i++;
1835 }
1836                 
1837         if(i == 2)
1838         {
1839         if(face2->v4)
1840         {
1841         collpair->ap1 = face1->v1;
1842         collpair->ap2 = face1->v2;
1843         collpair->ap3 = face1->v3;
1844                                 
1845         collpair->bp1 = face2->v3;
1846         collpair->bp2 = face2->v4;
1847         collpair->bp3 = face2->v1;
1848 }
1849         else
1850         i+=2;
1851 }
1852                 
1853         if(i == 3)
1854         {
1855         if((face1->v4)&&(face2->v4))
1856         {
1857         collpair->ap1 = face1->v3;
1858         collpair->ap2 = face1->v4;
1859         collpair->ap3 = face1->v1;
1860                                 
1861         collpair->bp1 = face2->v3;
1862         collpair->bp2 = face2->v4;
1863         collpair->bp3 = face2->v1;
1864 }
1865         else
1866         i++;
1867 }
1868                 
1869                 // calc SIPcode (?)
1870                 
1871         if(i < 4)
1872         {
1873                         // calc distance + normal       
1874         distance = plNearestPoints(
1875         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);
1876                         
1877         if (distance <= (epsilon + ALMOST_ZERO))
1878         {
1879                                 // printf("dist: %f\n", (float)distance);
1880                                 
1881                                 // collpair->face1 = tree1->tri_index;
1882                                 // collpair->face2 = tree2->tri_index;
1883                                 
1884                                 // VECCOPY(collpair->normal, collpair->vector);
1885                                 // Normalize(collpair->normal);
1886                                 
1887                                 // collpair->distance = distance;
1888                                 
1889 }
1890         else
1891         {
1892         MEM_freeN(collpair);
1893 }
1894 }
1895         else
1896         {
1897         MEM_freeN(collpair);
1898 }
1899 }
1900         */
1901 }
1902
1903 int collisions_are_edges_adjacent(ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair)
1904 {
1905         Cloth *cloth1, *cloth2;
1906         ClothVertex *verts1, *verts2;
1907         float temp[3];
1908          /*
1909         cloth1 = clmd->clothObject;
1910         cloth2 = coll_clmd->clothObject;
1911         
1912         verts1 = cloth1->verts;
1913         verts2 = cloth2->verts;
1914         
1915         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold);
1916         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1917                 return 1;
1918         
1919         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold);
1920         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1921                 return 1;
1922         
1923         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold);
1924         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1925                 return 1;
1926         
1927         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold);
1928         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1929                 return 1;
1930                 */
1931         return 0;
1932 }
1933
1934
1935 void collisions_collision_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
1936 {
1937         /*
1938         EdgeCollPair edgecollpair;
1939         Cloth *cloth1=NULL, *cloth2=NULL;
1940         MFace *face1=NULL, *face2=NULL;
1941         ClothVertex *verts1=NULL, *verts2=NULL;
1942         double distance = 0;
1943         float epsilon = clmd->coll_parms.epsilon;
1944         unsigned int i = 0, j = 0, k = 0;
1945         int numsolutions = 0;
1946         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
1947         
1948         cloth1 = clmd->clothObject;
1949         cloth2 = coll_clmd->clothObject;
1950         
1951         verts1 = cloth1->verts;
1952         verts2 = cloth2->verts;
1953
1954         face1 = &(cloth1->mfaces[tree1->tri_index]);
1955         face2 = &(cloth2->mfaces[tree2->tri_index]);
1956         
1957         for( i = 0; i < 5; i++)
1958         {
1959         if(i == 0) 
1960         {
1961         edgecollpair.p11 = face1->v1;
1962         edgecollpair.p12 = face1->v2;
1963 }
1964         else if(i == 1) 
1965         {
1966         edgecollpair.p11 = face1->v2;
1967         edgecollpair.p12 = face1->v3;
1968 }
1969         else if(i == 2) 
1970         {
1971         if(face1->v4) 
1972         {
1973         edgecollpair.p11 = face1->v3;
1974         edgecollpair.p12 = face1->v4;
1975 }
1976         else 
1977         {
1978         edgecollpair.p11 = face1->v3;
1979         edgecollpair.p12 = face1->v1;
1980         i+=5; // get out of here after this edge pair is handled
1981 }
1982 }
1983         else if(i == 3) 
1984         {
1985         if(face1->v4) 
1986         {
1987         edgecollpair.p11 = face1->v4;
1988         edgecollpair.p12 = face1->v1;
1989 }       
1990         else
1991         continue;
1992 }
1993         else
1994         {
1995         edgecollpair.p11 = face1->v3;
1996         edgecollpair.p12 = face1->v1;
1997 }
1998
1999                 
2000         for( j = 0; j < 5; j++)
2001         {
2002         if(j == 0)
2003         {
2004         edgecollpair.p21 = face2->v1;
2005         edgecollpair.p22 = face2->v2;
2006 }
2007         else if(j == 1)
2008         {
2009         edgecollpair.p21 = face2->v2;
2010         edgecollpair.p22 = face2->v3;
2011 }
2012         else if(j == 2)
2013         {
2014         if(face2->v4) 
2015         {
2016         edgecollpair.p21 = face2->v3;
2017         edgecollpair.p22 = face2->v4;
2018 }
2019         else 
2020         {
2021         edgecollpair.p21 = face2->v3;
2022         edgecollpair.p22 = face2->v1;
2023 }
2024 }
2025         else if(j == 3)
2026         {
2027         if(face2->v4) 
2028         {
2029         edgecollpair.p21 = face2->v4;
2030         edgecollpair.p22 = face2->v1;
2031 }
2032         else
2033         continue;
2034 }
2035         else
2036         {
2037         edgecollpair.p21 = face2->v3;
2038         edgecollpair.p22 = face2->v1;
2039 }
2040                         
2041                         
2042         if(!collisions_are_edges_adjacent(clmd, coll_clmd, &edgecollpair))
2043         {
2044         VECSUB(a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold);
2045         VECSUB(b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v);
2046         VECSUB(c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold);
2047         VECSUB(d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v);
2048         VECSUB(e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold);
2049         VECSUB(f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v);
2050                                 
2051         numsolutions = collisions_get_collision_time(a, b, c, d, e, f, solution);
2052                                 
2053         for (k = 0; k < numsolutions; k++) 
2054         {                                                               
2055         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
2056         {
2057         float out_collisionTime = solution[k];
2058                                                 
2059                                                 // TODO: check for collisions 
2060                                                 
2061                                                 // TODO: put into (edge) collision list
2062                                                 
2063         printf("Moving edge found!\n");
2064 }
2065 }
2066 }
2067 }
2068 }       
2069         */      
2070 }
2071
2072 void collisions_collision_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
2073 {
2074         /*
2075         CollPair collpair;
2076         Cloth *cloth1=NULL, *cloth2=NULL;
2077         MFace *face1=NULL, *face2=NULL;
2078         ClothVertex *verts1=NULL, *verts2=NULL;
2079         double distance = 0;
2080         float epsilon = clmd->coll_parms.epsilon;
2081         unsigned int i = 0, j = 0, k = 0;
2082         int numsolutions = 0;
2083         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
2084
2085         for(i = 0; i < 2; i++)
2086         {               
2087         cloth1 = clmd->clothObject;
2088         cloth2 = coll_clmd->clothObject;
2089                 
2090         verts1 = cloth1->verts;
2091         verts2 = cloth2->verts;
2092         
2093         face1 = &(cloth1->mfaces[tree1->tri_index]);
2094         face2 = &(cloth2->mfaces[tree2->tri_index]);
2095                 
2096                 // check all possible pairs of triangles
2097         if(i == 0)
2098         {
2099         collpair.ap1 = face1->v1;
2100         collpair.ap2 = face1->v2;
2101         collpair.ap3 = face1->v3;
2102                         
2103         collpair.pointsb[0] = face2->v1;
2104         collpair.pointsb[1] = face2->v2;
2105         collpair.pointsb[2] = face2->v3;
2106         collpair.pointsb[3] = face2->v4;
2107 }
2108                 
2109         if(i == 1)
2110         {
2111         if(face1->v4)
2112         {
2113         collpair.ap1 = face1->v3;
2114         collpair.ap2 = face1->v4;
2115         collpair.ap3 = face1->v1;
2116                                 
2117         collpair.pointsb[0] = face2->v1;
2118         collpair.pointsb[1] = face2->v2;
2119         collpair.pointsb[2] = face2->v3;
2120         collpair.pointsb[3] = face2->v4;
2121 }
2122         else
2123         i++;
2124 }
2125                 
2126                 // calc SIPcode (?)
2127                 
2128         if(i < 2)
2129         {
2130         VECSUB(a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold);
2131         VECSUB(b, verts1[collpair.ap2].v, verts1[collpair.ap1].v);
2132         VECSUB(c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold);
2133         VECSUB(d, verts1[collpair.ap3].v, verts1[collpair.ap1].v);
2134                                 
2135         for(j = 0; j < 4; j++)
2136         {                                       
2137         if((j==3) && !(face2->v4))
2138         break;
2139                                 
2140         VECSUB(e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold);
2141         VECSUB(f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v);
2142                                 
2143         numsolutions = collisions_get_collision_time(a, b, c, d, e, f, solution);
2144                                 
2145         for (k = 0; k < numsolutions; k++) 
2146         {                                                               
2147         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
2148         {
2149         float out_collisionTime = solution[k];
2150                                                 
2151                                                 // TODO: check for collisions 
2152                                                 
2153                                                 // TODO: put into (point-face) collision list
2154                                                 
2155         printf("Moving found!\n");
2156                                                 
2157 }
2158 }
2159                                 
2160                                 // TODO: check borders for collisions
2161 }
2162                         
2163 }
2164 }
2165         */
2166 }
2167
2168
2169 // move collision objects forward in time and update static bounding boxes
2170 void collisions_update_collision_objects(float step)
2171 {
2172         Base *base=NULL;
2173         ClothModifierData *coll_clmd=NULL;
2174         Object *coll_ob=NULL;
2175         unsigned int i=0;
2176         /*
2177         // search all objects for collision object
2178         for (base = G.scene->base.first; base; base = base->next)
2179         {
2180
2181                 coll_ob = base->object;
2182                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
2183                 if (!coll_clmd)
2184                         continue;
2185
2186                 // if collision object go on
2187                 if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
2188                 {
2189                         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
2190                         {
2191                                 Cloth *coll_cloth = coll_clmd->clothObject;
2192                                 BVH *coll_bvh = coll_clmd->clothObject->tree;
2193                                 unsigned int coll_numverts = coll_cloth->numverts;
2194
2195                                 // update position of collision object
2196                                 for(i = 0; i < coll_numverts; i++)
2197                                 {
2198                                         VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
2199
2200                                         VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
2201                                         
2202                                         // no dt here because of float rounding errors
2203                                         VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
2204                                 }
2205                                 
2206                                 // update BVH of collision object
2207                                 // bvh_update(coll_clmd, coll_bvh, 0); // 0 means STATIC, 1 means MOVING 
2208                         }
2209                         else
2210                                 printf ("collisions_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
2211                 }
2212         }
2213         */
2214 }
2215
2216
2217 void collisions_collision_moving(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
2218 {
2219         /*
2220         // TODO: check for adjacent
2221         collisions_collision_moving_edges(clmd, coll_clmd, tree1, tree2);
2222         
2223         collisions_collision_moving_tris(clmd, coll_clmd, tree1, tree2);
2224         collisions_collision_moving_tris(coll_clmd, clmd, tree2, tree1);
2225         */
2226 }
2227
2228 // cloth - object collisions
2229 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, float prevstep, float dt)
2230 {
2231         
2232         Base *base = NULL;
2233         CollisionModifierData *collmd = NULL;
2234         Cloth *cloth = NULL;
2235         Object *ob2 = NULL;
2236         BVH *bvh1 = NULL, *bvh2 = NULL;
2237         LinkNode *collision_list = NULL; 
2238         unsigned int i = 0, j = 0;
2239         int collisions = 0, count = 0;
2240         float (*current_x)[3];
2241
2242         if (!(((Cloth *)clmd->clothObject)->tree))
2243         {
2244                 printf("No BVH found\n");
2245                 return 0;
2246         }
2247         
2248         cloth = clmd->clothObject;
2249         bvh1 = cloth->tree;
2250         
2251         ////////////////////////////////////////////////////////////
2252         // static collisions
2253         ////////////////////////////////////////////////////////////
2254         /*
2255         // update cloth bvh
2256         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)
2257         
2258         // check all collision objects
2259         for (base = G.scene->base.first; base; base = base->next)
2260         {
2261                 ob2 = base->object;
2262                 collmd = (CollisionModifierData *) modifiers_findByType (ob2, eModifierType_Collision);
2263                 
2264                 if (!collmd)
2265                         continue;
2266                 
2267                 // check if there is a bounding volume hierarchy
2268                 if (collmd->tree) 
2269                 {                       
2270                         bvh2 = collmd->tree;
2271                         
2272                         // update position + bvh of collision object
2273                         collision_move_object(collmd, step, prevstep);
2274                         bvh_update_from_mvert(collmd->tree, collmd->current_x, collmd->numverts, NULL, 0);
2275                         
2276                         // fill collision list 
2277                         collisions += bvh_traverse(bvh1->root, bvh2->root, &collision_list);
2278                         
2279                         // free collision list
2280                         if(collision_list)
2281                         {
2282                                 LinkNode *search = collision_list;
2283                                 
2284                                 while(search)
2285                                 {
2286                                         CollisionPair *coll_pair = search->link;
2287                                         
2288                                         MEM_freeN(coll_pair);
2289                                         search = search->next;
2290                                 }
2291                                 BLI_linklist_free(collision_list,NULL);
2292         
2293                                 collision_list = NULL;
2294                         }
2295                 }
2296         }
2297         
2298         //////////////////////////////////////////////
2299         // update velocities + positions
2300         //////////////////////////////////////////////
2301         for(i = 0; i < cloth->numverts; i++)
2302         {
2303                 VECADD(cloth->current_x[i], cloth->current_xold[i], cloth->current_v[i]);
2304         }
2305         //////////////////////////////////////////////
2306         */
2307         // Test on *simple* selfcollisions
2308         collisions = 1;
2309         count = 0;
2310         current_x = cloth->current_x; // needed for openMP
2311         
2312 #pragma omp parallel for private(i,j, collisions) shared(current_x)
2313         for(count = 0; count < 6; count++)
2314         {
2315                 collisions = 0;
2316                 
2317                 for(i = 0; i < cloth->numverts; i++)
2318                 {
2319                         for(j = i + 1; j < cloth->numverts; j++)
2320                         {
2321                                 float temp[3];
2322                                 float length = 0;
2323                                 float mindistance = cloth->selftree->epsilon;
2324                                         
2325                                 if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL)
2326                                 {                       
2327                                         if((cloth->verts [i].goal >= SOFTGOALSNAP)
2328                                         && (cloth->verts [j].goal >= SOFTGOALSNAP))
2329                                         {
2330                                                 continue;
2331                                         }
2332                                 }
2333                                         
2334                                         // check for adjacent points
2335                                 if(BLI_edgehash_haskey ( cloth->edgehash, i, j ))
2336                                 {
2337                                         continue;
2338                                 }
2339                                         
2340                                 VECSUB(temp, current_x[i], current_x[j]);
2341                                         
2342                                 length = Normalize(temp);
2343                                         
2344                                 if(length < mindistance)
2345                                 {
2346                                         float correction = mindistance - length;
2347                                                 
2348                                         if((clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [i].goal >= SOFTGOALSNAP))
2349                                         {
2350                                                 VecMulf(temp, -correction);
2351                                                 VECADD(current_x[j], current_x[j], temp);
2352                                         }
2353                                         else if((clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (cloth->verts [j].goal >= SOFTGOALSNAP))
2354                                         {
2355                                                 VecMulf(temp, correction);
2356                                                 VECADD(current_x[i], current_x[i], temp);
2357                                         }
2358                                         else
2359                                         {
2360                                                 VecMulf(temp, -correction*0.5);
2361                                                 VECADD(current_x[j], current_x[j], temp);
2362                                                 
2363                                                 VECSUB(current_x[i], current_x[i], temp);       
2364                                         }
2365                                         
2366                                         collisions = 1;
2367                                 }
2368                         }
2369                 }
2370         }
2371         
2372         //////////////////////////////////////////////
2373         // SELFCOLLISIONS: update velocities
2374         //////////////////////////////////////////////
2375         for(i = 0; i < cloth->numverts; i++)
2376         {
2377                 VECSUB(cloth->current_v[i], cloth->current_x[i], cloth->current_xold[i]);
2378         }
2379         //////////////////////////////////////////////
2380         
2381         return 1;
2382 }