Win32: Use the nicer looking blender-drawn confirmation message box when
[blender.git] / source / blender / physics / intern / implicit_blender.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) Blender Foundation
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/physics/intern/implicit_blender.c
29  *  \ingroup bph
30  */
31
32 #include "implicit.h"
33
34 #ifdef IMPLICIT_SOLVER_BLENDER
35
36 #include "MEM_guardedalloc.h"
37
38 #include "DNA_scene_types.h"
39 #include "DNA_object_types.h"
40 #include "DNA_object_force_types.h"
41 #include "DNA_meshdata_types.h"
42 #include "DNA_texture_types.h"
43
44 #include "BLI_math.h"
45 #include "BLI_linklist.h"
46 #include "BLI_utildefines.h"
47
48 #include "BKE_cloth.h"
49 #include "BKE_collision.h"
50 #include "BKE_effect.h"
51
52 #include "BPH_mass_spring.h"
53
54 #ifdef __GNUC__
55 #  pragma GCC diagnostic ignored "-Wtype-limits"
56 #endif
57
58 #ifdef _OPENMP
59 #  define CLOTH_OPENMP_LIMIT 512
60 #endif
61
62 //#define DEBUG_TIME
63
64 #ifdef DEBUG_TIME
65 #       include "PIL_time.h"
66 #endif
67
68 static float I[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
69 static float ZERO[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
70
71 #if 0
72 #define C99
73 #ifdef C99
74 #defineDO_INLINE inline
75 #else
76 #defineDO_INLINE static
77 #endif
78 #endif  /* if 0 */
79
80 struct Cloth;
81
82 //////////////////////////////////////////
83 /* fast vector / matrix library, enhancements are welcome :) -dg */
84 /////////////////////////////////////////
85
86 /* DEFINITIONS */
87 typedef float lfVector[3];
88 typedef struct fmatrix3x3 {
89         float m[3][3]; /* 3x3 matrix */
90         unsigned int c, r; /* column and row number */
91         /* int pinned; // is this vertex allowed to move? */
92         float n1, n2, n3; /* three normal vectors for collision constrains */
93         unsigned int vcount; /* vertex count */
94         unsigned int scount; /* spring count */
95 } fmatrix3x3;
96
97 ///////////////////////////
98 // float[3] vector
99 ///////////////////////////
100 /* simple vector code */
101 /* STATUS: verified */
102 DO_INLINE void mul_fvector_S(float to[3], float from[3], float scalar)
103 {
104         to[0] = from[0] * scalar;
105         to[1] = from[1] * scalar;
106         to[2] = from[2] * scalar;
107 }
108 /* simple v^T * v product ("outer product") */
109 /* STATUS: HAS TO BE verified (*should* work) */
110 DO_INLINE void mul_fvectorT_fvector(float to[3][3], float vectorA[3], float vectorB[3])
111 {
112         mul_fvector_S(to[0], vectorB, vectorA[0]);
113         mul_fvector_S(to[1], vectorB, vectorA[1]);
114         mul_fvector_S(to[2], vectorB, vectorA[2]);
115 }
116 /* simple v^T * v product with scalar ("outer product") */
117 /* STATUS: HAS TO BE verified (*should* work) */
118 DO_INLINE void mul_fvectorT_fvectorS(float to[3][3], float vectorA[3], float vectorB[3], float aS)
119 {
120         mul_fvectorT_fvector(to, vectorA, vectorB);
121
122         mul_fvector_S(to[0], to[0], aS);
123         mul_fvector_S(to[1], to[1], aS);
124         mul_fvector_S(to[2], to[2], aS);
125 }
126
127 #if 0
128 /* printf vector[3] on console: for debug output */
129 static void print_fvector(float m3[3])
130 {
131         printf("%f\n%f\n%f\n\n", m3[0], m3[1], m3[2]);
132 }
133
134 ///////////////////////////
135 // long float vector float (*)[3]
136 ///////////////////////////
137 /* print long vector on console: for debug output */
138 DO_INLINE void print_lfvector(float (*fLongVector)[3], unsigned int verts)
139 {
140         unsigned int i = 0;
141         for (i = 0; i < verts; i++) {
142                 print_fvector(fLongVector[i]);
143         }
144 }
145 #endif
146
147 /* create long vector */
148 DO_INLINE lfVector *create_lfvector(unsigned int verts)
149 {
150         /* TODO: check if memory allocation was successful */
151         return  (lfVector *)MEM_callocN(verts * sizeof(lfVector), "cloth_implicit_alloc_vector");
152         // return (lfVector *)cloth_aligned_malloc(&MEMORY_BASE, verts * sizeof(lfVector));
153 }
154 /* delete long vector */
155 DO_INLINE void del_lfvector(float (*fLongVector)[3])
156 {
157         if (fLongVector != NULL) {
158                 MEM_freeN(fLongVector);
159                 // cloth_aligned_free(&MEMORY_BASE, fLongVector);
160         }
161 }
162 /* copy long vector */
163 DO_INLINE void cp_lfvector(float (*to)[3], float (*from)[3], unsigned int verts)
164 {
165         memcpy(to, from, verts * sizeof(lfVector));
166 }
167 /* init long vector with float[3] */
168 DO_INLINE void init_lfvector(float (*fLongVector)[3], float vector[3], unsigned int verts)
169 {
170         unsigned int i = 0;
171         for (i = 0; i < verts; i++) {
172                 copy_v3_v3(fLongVector[i], vector);
173         }
174 }
175 /* zero long vector with float[3] */
176 DO_INLINE void zero_lfvector(float (*to)[3], unsigned int verts)
177 {
178         memset(to, 0.0f, verts * sizeof(lfVector));
179 }
180 /* multiply long vector with scalar*/
181 DO_INLINE void mul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts)
182 {
183         unsigned int i = 0;
184
185         for (i = 0; i < verts; i++) {
186                 mul_fvector_S(to[i], fLongVector[i], scalar);
187         }
188 }
189 /* multiply long vector with scalar*/
190 /* A -= B * float */
191 DO_INLINE void submul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts)
192 {
193         unsigned int i = 0;
194         for (i = 0; i < verts; i++) {
195                 VECSUBMUL(to[i], fLongVector[i], scalar);
196         }
197 }
198 /* dot product for big vector */
199 DO_INLINE float dot_lfvector(float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
200 {
201         long i = 0;
202         float temp = 0.0;
203 // XXX brecht, disabled this for now (first schedule line was already disabled),
204 // due to non-commutative nature of floating point ops this makes the sim give
205 // different results each time you run it!
206 // schedule(guided, 2)
207 //#pragma omp parallel for reduction(+: temp) if (verts > CLOTH_OPENMP_LIMIT)
208         for (i = 0; i < (long)verts; i++) {
209                 temp += dot_v3v3(fLongVectorA[i], fLongVectorB[i]);
210         }
211         return temp;
212 }
213 /* A = B + C  --> for big vector */
214 DO_INLINE void add_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
215 {
216         unsigned int i = 0;
217
218         for (i = 0; i < verts; i++) {
219                 VECADD(to[i], fLongVectorA[i], fLongVectorB[i]);
220         }
221
222 }
223 /* A = B + C * float --> for big vector */
224 DO_INLINE void add_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts)
225 {
226         unsigned int i = 0;
227
228         for (i = 0; i < verts; i++) {
229                 VECADDS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
230
231         }
232 }
233 /* A = B * float + C * float --> for big vector */
234 DO_INLINE void add_lfvectorS_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float aS, float (*fLongVectorB)[3], float bS, unsigned int verts)
235 {
236         unsigned int i = 0;
237
238         for (i = 0; i < verts; i++) {
239                 VECADDSS(to[i], fLongVectorA[i], aS, fLongVectorB[i], bS);
240         }
241 }
242 /* A = B - C * float --> for big vector */
243 DO_INLINE void sub_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts)
244 {
245         unsigned int i = 0;
246         for (i = 0; i < verts; i++) {
247                 VECSUBS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
248         }
249
250 }
251 /* A = B - C --> for big vector */
252 DO_INLINE void sub_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
253 {
254         unsigned int i = 0;
255
256         for (i = 0; i < verts; i++) {
257                 sub_v3_v3v3(to[i], fLongVectorA[i], fLongVectorB[i]);
258         }
259
260 }
261 ///////////////////////////
262 // 3x3 matrix
263 ///////////////////////////
264 #if 0
265 /* printf 3x3 matrix on console: for debug output */
266 static void print_fmatrix(float m3[3][3])
267 {
268         printf("%f\t%f\t%f\n", m3[0][0], m3[0][1], m3[0][2]);
269         printf("%f\t%f\t%f\n", m3[1][0], m3[1][1], m3[1][2]);
270         printf("%f\t%f\t%f\n\n", m3[2][0], m3[2][1], m3[2][2]);
271 }
272
273 static void print_sparse_matrix(fmatrix3x3 *m)
274 {
275         if (m) {
276                 unsigned int i;
277                 for (i = 0; i < m[0].vcount + m[0].scount; i++) {
278                         printf("%d:\n", i);
279                         print_fmatrix(m[i].m);
280                 }
281         }
282 }
283 #endif
284
285 #if 0
286 static void print_lvector(lfVector *v, int numverts)
287 {
288         int i;
289         for (i = 0; i < numverts; ++i) {
290                 if (i > 0)
291                         printf("\n");
292
293                 printf("%f,\n", v[i][0]);
294                 printf("%f,\n", v[i][1]);
295                 printf("%f,\n", v[i][2]);
296         }
297 }
298 #endif
299
300 #if 0
301 static void print_bfmatrix(fmatrix3x3 *m)
302 {
303         int tot = m[0].vcount + m[0].scount;
304         int size = m[0].vcount * 3;
305         float *t = MEM_callocN(sizeof(float) * size*size, "bfmatrix");
306         int q, i, j;
307
308         for (q = 0; q < tot; ++q) {
309                 int k = 3 * m[q].r;
310                 int l = 3 * m[q].c;
311
312                 for (j = 0; j < 3; ++j) {
313                         for (i = 0; i < 3; ++i) {
314 //                              if (t[k + i + (l + j) * size] != 0.0f) {
315 //                                      printf("warning: overwriting value at %d, %d\n", m[q].r, m[q].c);
316 //                              }
317                                 if (k == l) {
318                                         t[k + i + (k + j) * size] += m[q].m[i][j];
319                                 }
320                                 else {
321                                         t[k + i + (l + j) * size] += m[q].m[i][j];
322                                         t[l + j + (k + i) * size] += m[q].m[j][i];
323                                 }
324                         }
325                 }
326         }
327
328         for (j = 0; j < size; ++j) {
329                 if (j > 0 && j % 3 == 0)
330                         printf("\n");
331
332                 for (i = 0; i < size; ++i) {
333                         if (i > 0 && i % 3 == 0)
334                                 printf("  ");
335
336                         implicit_print_matrix_elem(t[i + j * size]);
337                 }
338                 printf("\n");
339         }
340
341         MEM_freeN(t);
342 }
343 #endif
344
345 /* copy 3x3 matrix */
346 DO_INLINE void cp_fmatrix(float to[3][3], float from[3][3])
347 {
348         // memcpy(to, from, sizeof (float) * 9);
349         copy_v3_v3(to[0], from[0]);
350         copy_v3_v3(to[1], from[1]);
351         copy_v3_v3(to[2], from[2]);
352 }
353
354 /* copy 3x3 matrix */
355 DO_INLINE void initdiag_fmatrixS(float to[3][3], float aS)
356 {
357         cp_fmatrix(to, ZERO);
358
359         to[0][0] = aS;
360         to[1][1] = aS;
361         to[2][2] = aS;
362 }
363
364 #if 0
365 /* calculate determinant of 3x3 matrix */
366 DO_INLINE float det_fmatrix(float m[3][3])
367 {
368         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] -
369                 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];
370 }
371
372 DO_INLINE void inverse_fmatrix(float to[3][3], float from[3][3])
373 {
374         unsigned int i, j;
375         float d;
376
377         if ((d=det_fmatrix(from)) == 0) {
378                 printf("can't build inverse");
379                 exit(0);
380         }
381         for (i=0;i<3;i++) {
382                 for (j=0;j<3;j++) {
383                         int i1=(i+1)%3;
384                         int i2=(i+2)%3;
385                         int j1=(j+1)%3;
386                         int j2=(j+2)%3;
387                         /** Reverse indexes i&j to take transpose. */
388                         to[j][i] = (from[i1][j1]*from[i2][j2]-from[i1][j2]*from[i2][j1])/d;
389                         /**
390                          * <pre>
391                          * if (i == j) {
392                          *     to[i][j] = 1.0f / from[i][j];
393                          * }
394                          * else {
395                          *     to[i][j] = 0;
396                          * }
397                          * </pre>
398                          */
399                 }
400         }
401
402 }
403 #endif
404
405 /* 3x3 matrix multiplied by a scalar */
406 /* STATUS: verified */
407 DO_INLINE void mul_fmatrix_S(float matrix[3][3], float scalar)
408 {
409         mul_fvector_S(matrix[0], matrix[0], scalar);
410         mul_fvector_S(matrix[1], matrix[1], scalar);
411         mul_fvector_S(matrix[2], matrix[2], scalar);
412 }
413
414 /* a vector multiplied by a 3x3 matrix */
415 /* STATUS: verified */
416 DO_INLINE void mul_fvector_fmatrix(float *to, float *from, float matrix[3][3])
417 {
418         to[0] = matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
419         to[1] = matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
420         to[2] = matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
421 }
422
423 /* 3x3 matrix multiplied by a vector */
424 /* STATUS: verified */
425 DO_INLINE void mul_fmatrix_fvector(float *to, float matrix[3][3], float from[3])
426 {
427         to[0] = dot_v3v3(matrix[0], from);
428         to[1] = dot_v3v3(matrix[1], from);
429         to[2] = dot_v3v3(matrix[2], from);
430 }
431 /* 3x3 matrix addition with 3x3 matrix */
432 DO_INLINE void add_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
433 {
434         VECADD(to[0], matrixA[0], matrixB[0]);
435         VECADD(to[1], matrixA[1], matrixB[1]);
436         VECADD(to[2], matrixA[2], matrixB[2]);
437 }
438 /* A -= B*x + C*y (3x3 matrix sub-addition with 3x3 matrix) */
439 DO_INLINE void subadd_fmatrixS_fmatrixS(float to[3][3], float matrixA[3][3], float aS, float matrixB[3][3], float bS)
440 {
441         VECSUBADDSS(to[0], matrixA[0], aS, matrixB[0], bS);
442         VECSUBADDSS(to[1], matrixA[1], aS, matrixB[1], bS);
443         VECSUBADDSS(to[2], matrixA[2], aS, matrixB[2], bS);
444 }
445 /* A = B - C (3x3 matrix subtraction with 3x3 matrix) */
446 DO_INLINE void sub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
447 {
448         sub_v3_v3v3(to[0], matrixA[0], matrixB[0]);
449         sub_v3_v3v3(to[1], matrixA[1], matrixB[1]);
450         sub_v3_v3v3(to[2], matrixA[2], matrixB[2]);
451 }
452 /////////////////////////////////////////////////////////////////
453 // special functions
454 /////////////////////////////////////////////////////////////////
455 /* 3x3 matrix multiplied+added by a vector */
456 /* STATUS: verified */
457 DO_INLINE void muladd_fmatrix_fvector(float to[3], float matrix[3][3], float from[3])
458 {
459         to[0] += dot_v3v3(matrix[0], from);
460         to[1] += dot_v3v3(matrix[1], from);
461         to[2] += dot_v3v3(matrix[2], from);
462 }
463
464 DO_INLINE void muladd_fmatrixT_fvector(float to[3], float matrix[3][3], float from[3])
465 {
466         to[0] += matrix[0][0] * from[0] + matrix[1][0] * from[1] + matrix[2][0] * from[2];
467         to[1] += matrix[0][1] * from[0] + matrix[1][1] * from[1] + matrix[2][1] * from[2];
468         to[2] += matrix[0][2] * from[0] + matrix[1][2] * from[1] + matrix[2][2] * from[2];
469 }
470
471 BLI_INLINE void outerproduct(float r[3][3], const float a[3], const float b[3])
472 {
473         mul_v3_v3fl(r[0], a, b[0]);
474         mul_v3_v3fl(r[1], a, b[1]);
475         mul_v3_v3fl(r[2], a, b[2]);
476 }
477
478 BLI_INLINE void cross_m3_v3m3(float r[3][3], const float v[3], float m[3][3])
479 {
480         cross_v3_v3v3(r[0], v, m[0]);
481         cross_v3_v3v3(r[1], v, m[1]);
482         cross_v3_v3v3(r[2], v, m[2]);
483 }
484
485 BLI_INLINE void cross_v3_identity(float r[3][3], const float v[3])
486 {
487         r[0][0] = 0.0f;         r[1][0] = v[2];         r[2][0] = -v[1];
488         r[0][1] = -v[2];        r[1][1] = 0.0f;         r[2][1] = v[0];
489         r[0][2] = v[1];         r[1][2] = -v[0];        r[2][2] = 0.0f;
490 }
491
492 BLI_INLINE void madd_m3_m3fl(float r[3][3], float m[3][3], float f)
493 {
494         r[0][0] += m[0][0] * f;
495         r[0][1] += m[0][1] * f;
496         r[0][2] += m[0][2] * f;
497         r[1][0] += m[1][0] * f;
498         r[1][1] += m[1][1] * f;
499         r[1][2] += m[1][2] * f;
500         r[2][0] += m[2][0] * f;
501         r[2][1] += m[2][1] * f;
502         r[2][2] += m[2][2] * f;
503 }
504
505 BLI_INLINE void madd_m3_m3m3fl(float r[3][3], float a[3][3], float b[3][3], float f)
506 {
507         r[0][0] = a[0][0] + b[0][0] * f;
508         r[0][1] = a[0][1] + b[0][1] * f;
509         r[0][2] = a[0][2] + b[0][2] * f;
510         r[1][0] = a[1][0] + b[1][0] * f;
511         r[1][1] = a[1][1] + b[1][1] * f;
512         r[1][2] = a[1][2] + b[1][2] * f;
513         r[2][0] = a[2][0] + b[2][0] * f;
514         r[2][1] = a[2][1] + b[2][1] * f;
515         r[2][2] = a[2][2] + b[2][2] * f;
516 }
517 /////////////////////////////////////////////////////////////////
518
519 ///////////////////////////
520 // SPARSE SYMMETRIC big matrix with 3x3 matrix entries
521 ///////////////////////////
522 /* printf a big matrix on console: for debug output */
523 #if 0
524 static void print_bfmatrix(fmatrix3x3 *m3)
525 {
526         unsigned int i = 0;
527
528         for (i = 0; i < m3[0].vcount + m3[0].scount; i++)
529         {
530                 print_fmatrix(m3[i].m);
531         }
532 }
533 #endif
534
535 BLI_INLINE void init_fmatrix(fmatrix3x3 *matrix, int r, int c)
536 {
537         matrix->r = r;
538         matrix->c = c;
539 }
540
541 /* create big matrix */
542 DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs)
543 {
544         // TODO: check if memory allocation was successful */
545         fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN(sizeof(fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix");
546         int i;
547
548         temp[0].vcount = verts;
549         temp[0].scount = springs;
550
551         /* vertex part of the matrix is diagonal blocks */
552         for (i = 0; i < verts; ++i) {
553                 init_fmatrix(temp + i, i, i);
554         }
555
556         return temp;
557 }
558 /* delete big matrix */
559 DO_INLINE void del_bfmatrix(fmatrix3x3 *matrix)
560 {
561         if (matrix != NULL) {
562                 MEM_freeN(matrix);
563         }
564 }
565
566 /* copy big matrix */
567 DO_INLINE void cp_bfmatrix(fmatrix3x3 *to, fmatrix3x3 *from)
568 {
569         // TODO bounds checking
570         memcpy(to, from, sizeof(fmatrix3x3) * (from[0].vcount+from[0].scount));
571 }
572
573 /* init big matrix */
574 // slow in parallel
575 DO_INLINE void init_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
576 {
577         unsigned int i;
578
579         for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) {
580                 cp_fmatrix(matrix[i].m, m3);
581         }
582 }
583
584 /* init the diagonal of big matrix */
585 // slow in parallel
586 DO_INLINE void initdiag_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
587 {
588         unsigned int i, j;
589         float tmatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
590
591         for (i = 0; i < matrix[0].vcount; i++) {
592                 cp_fmatrix(matrix[i].m, m3);
593         }
594         for (j = matrix[0].vcount; j < matrix[0].vcount+matrix[0].scount; j++) {
595                 cp_fmatrix(matrix[j].m, tmatrix);
596         }
597 }
598
599 /* SPARSE SYMMETRIC multiply big matrix with long vector*/
600 /* STATUS: verified */
601 DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector *fLongVector)
602 {
603         unsigned int i = 0;
604         unsigned int vcount = from[0].vcount;
605         lfVector *temp = create_lfvector(vcount);
606
607         zero_lfvector(to, vcount);
608
609 #pragma omp parallel sections private(i) if (vcount > CLOTH_OPENMP_LIMIT)
610         {
611 #pragma omp section
612                 {
613                         for (i = from[0].vcount; i < from[0].vcount+from[0].scount; i++) {
614                                 /* This is the lower triangle of the sparse matrix,
615                                  * therefore multiplication occurs with transposed submatrices. */
616                                 muladd_fmatrixT_fvector(to[from[i].c], from[i].m, fLongVector[from[i].r]);
617                         }
618                 }
619 #pragma omp section
620                 {
621                         for (i = 0; i < from[0].vcount+from[0].scount; i++) {
622                                 muladd_fmatrix_fvector(temp[from[i].r], from[i].m, fLongVector[from[i].c]);
623                         }
624                 }
625         }
626         add_lfvector_lfvector(to, to, temp, from[0].vcount);
627
628         del_lfvector(temp);
629 }
630
631 /* SPARSE SYMMETRIC sub big matrix with big matrix*/
632 /* A -= B * float + C * float --> for big matrix */
633 /* VERIFIED */
634 DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, float aS,  fmatrix3x3 *matrix, float bS)
635 {
636         unsigned int i = 0;
637
638         /* process diagonal elements */
639         for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) {
640                 subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS);
641         }
642
643 }
644
645 ///////////////////////////////////////////////////////////////////
646 // simulator start
647 ///////////////////////////////////////////////////////////////////
648
649 typedef struct Implicit_Data  {
650         /* inputs */
651         fmatrix3x3 *bigI;                       /* identity (constant) */
652         fmatrix3x3 *tfm;                        /* local coordinate transform */
653         fmatrix3x3 *M;                          /* masses */
654         lfVector *F;                            /* forces */
655         fmatrix3x3 *dFdV, *dFdX;        /* force jacobians */
656         int num_blocks;                         /* number of off-diagonal blocks (springs) */
657
658         /* motion state data */
659         lfVector *X, *Xnew;                     /* positions */
660         lfVector *V, *Vnew;                     /* velocities */
661
662         /* internal solver data */
663         lfVector *B;                            /* B for A*dV = B */
664         fmatrix3x3 *A;                          /* A for A*dV = B */
665
666         lfVector *dV;                           /* velocity change (solution of A*dV = B) */
667         lfVector *z;                            /* target velocity in constrained directions */
668         fmatrix3x3 *S;                          /* filtering matrix for constraints */
669         fmatrix3x3 *P, *Pinv;           /* pre-conditioning matrix */
670 } Implicit_Data;
671
672 Implicit_Data *BPH_mass_spring_solver_create(int numverts, int numsprings)
673 {
674         Implicit_Data *id = (Implicit_Data *)MEM_callocN(sizeof(Implicit_Data), "implicit vecmat");
675
676         /* process diagonal elements */
677         id->tfm = create_bfmatrix(numverts, 0);
678         id->A = create_bfmatrix(numverts, numsprings);
679         id->dFdV = create_bfmatrix(numverts, numsprings);
680         id->dFdX = create_bfmatrix(numverts, numsprings);
681         id->S = create_bfmatrix(numverts, 0);
682         id->Pinv = create_bfmatrix(numverts, numsprings);
683         id->P = create_bfmatrix(numverts, numsprings);
684         id->bigI = create_bfmatrix(numverts, numsprings); // TODO 0 springs
685         id->M = create_bfmatrix(numverts, numsprings);
686         id->X = create_lfvector(numverts);
687         id->Xnew = create_lfvector(numverts);
688         id->V = create_lfvector(numverts);
689         id->Vnew = create_lfvector(numverts);
690         id->F = create_lfvector(numverts);
691         id->B = create_lfvector(numverts);
692         id->dV = create_lfvector(numverts);
693         id->z = create_lfvector(numverts);
694
695         initdiag_bfmatrix(id->bigI, I);
696
697         return id;
698 }
699
700 void BPH_mass_spring_solver_free(Implicit_Data *id)
701 {
702         del_bfmatrix(id->tfm);
703         del_bfmatrix(id->A);
704         del_bfmatrix(id->dFdV);
705         del_bfmatrix(id->dFdX);
706         del_bfmatrix(id->S);
707         del_bfmatrix(id->P);
708         del_bfmatrix(id->Pinv);
709         del_bfmatrix(id->bigI);
710         del_bfmatrix(id->M);
711
712         del_lfvector(id->X);
713         del_lfvector(id->Xnew);
714         del_lfvector(id->V);
715         del_lfvector(id->Vnew);
716         del_lfvector(id->F);
717         del_lfvector(id->B);
718         del_lfvector(id->dV);
719         del_lfvector(id->z);
720
721         MEM_freeN(id);
722 }
723
724 /* ==== Transformation from/to root reference frames ==== */
725
726 BLI_INLINE void world_to_root_v3(Implicit_Data *data, int index, float r[3], const float v[3])
727 {
728         copy_v3_v3(r, v);
729         mul_transposed_m3_v3(data->tfm[index].m, r);
730 }
731
732 BLI_INLINE void root_to_world_v3(Implicit_Data *data, int index, float r[3], const float v[3])
733 {
734         mul_v3_m3v3(r, data->tfm[index].m, v);
735 }
736
737 BLI_INLINE void world_to_root_m3(Implicit_Data *data, int index, float r[3][3], float m[3][3])
738 {
739         float trot[3][3];
740         copy_m3_m3(trot, data->tfm[index].m);
741         transpose_m3(trot);
742         mul_m3_m3m3(r, trot, m);
743 }
744
745 BLI_INLINE void root_to_world_m3(Implicit_Data *data, int index, float r[3][3], float m[3][3])
746 {
747         mul_m3_m3m3(r, data->tfm[index].m, m);
748 }
749
750 /* ================================ */
751
752 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
753 {
754         unsigned int i=0;
755
756         for (i = 0; i < S[0].vcount; i++) {
757                 mul_m3_v3(S[i].m, V[S[i].r]);
758         }
759 }
760
761 #if 0 /* this version of the CG algorithm does not work very well with partial constraints (where S has non-zero elements) */
762 static int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S)
763 {
764         // Solves for unknown X in equation AX=B
765         unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
766         float conjgrad_epsilon=0.0001f /* , conjgrad_lasterror=0 */ /* UNUSED */;
767         lfVector *q, *d, *tmp, *r;
768         float s, starget, a, s_prev;
769         unsigned int numverts = lA[0].vcount;
770         q = create_lfvector(numverts);
771         d = create_lfvector(numverts);
772         tmp = create_lfvector(numverts);
773         r = create_lfvector(numverts);
774
775         // zero_lfvector(ldV, CLOTHPARTICLES);
776         filter(ldV, S);
777
778         add_lfvector_lfvector(ldV, ldV, z, numverts);
779
780         // r = B - Mul(tmp, A, X);    // just use B if X known to be zero
781         cp_lfvector(r, lB, numverts);
782         mul_bfmatrix_lfvector(tmp, lA, ldV);
783         sub_lfvector_lfvector(r, r, tmp, numverts);
784
785         filter(r, S);
786
787         cp_lfvector(d, r, numverts);
788
789         s = dot_lfvector(r, r, numverts);
790         starget = s * sqrtf(conjgrad_epsilon);
791
792         while (s>starget && conjgrad_loopcount < conjgrad_looplimit) {
793                 // Mul(q, A, d); // q = A*d;
794                 mul_bfmatrix_lfvector(q, lA, d);
795
796                 filter(q, S);
797
798                 a = s/dot_lfvector(d, q, numverts);
799
800                 // X = X + d*a;
801                 add_lfvector_lfvectorS(ldV, ldV, d, a, numverts);
802
803                 // r = r - q*a;
804                 sub_lfvector_lfvectorS(r, r, q, a, numverts);
805
806                 s_prev = s;
807                 s = dot_lfvector(r, r, numverts);
808
809                 //d = r+d*(s/s_prev);
810                 add_lfvector_lfvectorS(d, r, d, (s/s_prev), numverts);
811
812                 filter(d, S);
813
814                 conjgrad_loopcount++;
815         }
816         /* conjgrad_lasterror = s; */ /* UNUSED */
817
818         del_lfvector(q);
819         del_lfvector(d);
820         del_lfvector(tmp);
821         del_lfvector(r);
822         // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount);
823
824         return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
825 }
826 #endif
827
828 static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, ImplicitSolverResult *result)
829 {
830         // Solves for unknown X in equation AX=B
831         unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
832         float conjgrad_epsilon=0.01f;
833
834         unsigned int numverts = lA[0].vcount;
835         lfVector *fB = create_lfvector(numverts);
836         lfVector *AdV = create_lfvector(numverts);
837         lfVector *r = create_lfvector(numverts);
838         lfVector *c = create_lfvector(numverts);
839         lfVector *q = create_lfvector(numverts);
840         lfVector *s = create_lfvector(numverts);
841         float bnorm2, delta_new, delta_old, delta_target, alpha;
842
843         cp_lfvector(ldV, z, numverts);
844
845         /* d0 = filter(B)^T * P * filter(B) */
846         cp_lfvector(fB, lB, numverts);
847         filter(fB, S);
848         bnorm2 = dot_lfvector(fB, fB, numverts);
849         delta_target = conjgrad_epsilon*conjgrad_epsilon * bnorm2;
850
851         /* r = filter(B - A * dV) */
852         mul_bfmatrix_lfvector(AdV, lA, ldV);
853         sub_lfvector_lfvector(r, lB, AdV, numverts);
854         filter(r, S);
855
856         /* c = filter(P^-1 * r) */
857         cp_lfvector(c, r, numverts);
858         filter(c, S);
859
860         /* delta = r^T * c */
861         delta_new = dot_lfvector(r, c, numverts);
862
863 #ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT
864         printf("==== A ====\n");
865         print_bfmatrix(lA);
866         printf("==== z ====\n");
867         print_lvector(z, numverts);
868         printf("==== B ====\n");
869         print_lvector(lB, numverts);
870         printf("==== S ====\n");
871         print_bfmatrix(S);
872 #endif
873
874         while (delta_new > delta_target && conjgrad_loopcount < conjgrad_looplimit) {
875                 mul_bfmatrix_lfvector(q, lA, c);
876                 filter(q, S);
877
878                 alpha = delta_new / dot_lfvector(c, q, numverts);
879
880                 add_lfvector_lfvectorS(ldV, ldV, c, alpha, numverts);
881
882                 add_lfvector_lfvectorS(r, r, q, -alpha, numverts);
883
884                 /* s = P^-1 * r */
885                 cp_lfvector(s, r, numverts);
886                 delta_old = delta_new;
887                 delta_new = dot_lfvector(r, s, numverts);
888
889                 add_lfvector_lfvectorS(c, s, c, delta_new / delta_old, numverts);
890                 filter(c, S);
891
892                 conjgrad_loopcount++;
893         }
894
895 #ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT
896         printf("==== dV ====\n");
897         print_lvector(ldV, numverts);
898         printf("========\n");
899 #endif
900
901         del_lfvector(fB);
902         del_lfvector(AdV);
903         del_lfvector(r);
904         del_lfvector(c);
905         del_lfvector(q);
906         del_lfvector(s);
907         // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount);
908
909         result->status = conjgrad_loopcount < conjgrad_looplimit ? BPH_SOLVER_SUCCESS : BPH_SOLVER_NO_CONVERGENCE;
910         result->iterations = conjgrad_loopcount;
911         result->error = bnorm2 > 0.0f ? sqrtf(delta_new / bnorm2) : 0.0f;
912
913         return conjgrad_loopcount < conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
914 }
915
916 #if 0
917 // block diagonalizer
918 DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv)
919 {
920         unsigned int i = 0;
921
922         // Take only the diagonal blocks of A
923 // #pragma omp parallel for private(i) if (lA[0].vcount > CLOTH_OPENMP_LIMIT)
924         for (i = 0; i<lA[0].vcount; i++) {
925                 // block diagonalizer
926                 cp_fmatrix(P[i].m, lA[i].m);
927                 inverse_fmatrix(Pinv[i].m, P[i].m);
928
929         }
930 }
931
932 #if 0
933 // version 1.3
934 static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
935 {
936         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
937         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0;
938         float conjgrad_epsilon=0.0001; // 0.2 is dt for steps=5
939         lfVector *r = create_lfvector(numverts);
940         lfVector *p = create_lfvector(numverts);
941         lfVector *s = create_lfvector(numverts);
942         lfVector *h = create_lfvector(numverts);
943
944         BuildPPinv(lA, P, Pinv);
945
946         filter(dv, S);
947         add_lfvector_lfvector(dv, dv, z, numverts);
948
949         mul_bfmatrix_lfvector(r, lA, dv);
950         sub_lfvector_lfvector(r, lB, r, numverts);
951         filter(r, S);
952
953         mul_prevfmatrix_lfvector(p, Pinv, r);
954         filter(p, S);
955
956         deltaNew = dot_lfvector(r, p, numverts);
957
958         delta0 = deltaNew * sqrt(conjgrad_epsilon);
959
960 #ifdef DEBUG_TIME
961         double start = PIL_check_seconds_timer();
962 #endif
963
964         while ((deltaNew > delta0) && (iterations < conjgrad_looplimit))
965         {
966                 iterations++;
967
968                 mul_bfmatrix_lfvector(s, lA, p);
969                 filter(s, S);
970
971                 alpha = deltaNew / dot_lfvector(p, s, numverts);
972
973                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
974
975                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
976
977                 mul_prevfmatrix_lfvector(h, Pinv, r);
978                 filter(h, S);
979
980                 deltaOld = deltaNew;
981
982                 deltaNew = dot_lfvector(r, h, numverts);
983
984                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
985
986                 filter(p, S);
987
988         }
989
990 #ifdef DEBUG_TIME
991         double end = PIL_check_seconds_timer();
992         printf("cg_filtered_pre time: %f\n", (float)(end - start));
993 #endif
994
995         del_lfvector(h);
996         del_lfvector(s);
997         del_lfvector(p);
998         del_lfvector(r);
999
1000         printf("iterations: %d\n", iterations);
1001
1002         return iterations<conjgrad_looplimit;
1003 }
1004 #endif
1005
1006 // version 1.4
1007 static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *bigI)
1008 {
1009         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
1010         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0, tol = 0;
1011         lfVector *r = create_lfvector(numverts);
1012         lfVector *p = create_lfvector(numverts);
1013         lfVector *s = create_lfvector(numverts);
1014         lfVector *h = create_lfvector(numverts);
1015         lfVector *bhat = create_lfvector(numverts);
1016         lfVector *btemp = create_lfvector(numverts);
1017
1018         BuildPPinv(lA, P, Pinv);
1019
1020         initdiag_bfmatrix(bigI, I);
1021         sub_bfmatrix_Smatrix(bigI, bigI, S);
1022
1023         // x = Sx_0+(I-S)z
1024         filter(dv, S);
1025         add_lfvector_lfvector(dv, dv, z, numverts);
1026
1027         // b_hat = S(b-A(I-S)z)
1028         mul_bfmatrix_lfvector(r, lA, z);
1029         mul_bfmatrix_lfvector(bhat, bigI, r);
1030         sub_lfvector_lfvector(bhat, lB, bhat, numverts);
1031
1032         // r = S(b-Ax)
1033         mul_bfmatrix_lfvector(r, lA, dv);
1034         sub_lfvector_lfvector(r, lB, r, numverts);
1035         filter(r, S);
1036
1037         // p = SP^-1r
1038         mul_prevfmatrix_lfvector(p, Pinv, r);
1039         filter(p, S);
1040
1041         // delta0 = bhat^TP^-1bhat
1042         mul_prevfmatrix_lfvector(btemp, Pinv, bhat);
1043         delta0 = dot_lfvector(bhat, btemp, numverts);
1044
1045         // deltaNew = r^TP
1046         deltaNew = dot_lfvector(r, p, numverts);
1047
1048 #if 0
1049         filter(dv, S);
1050         add_lfvector_lfvector(dv, dv, z, numverts);
1051
1052         mul_bfmatrix_lfvector(r, lA, dv);
1053         sub_lfvector_lfvector(r, lB, r, numverts);
1054         filter(r, S);
1055
1056         mul_prevfmatrix_lfvector(p, Pinv, r);
1057         filter(p, S);
1058
1059         deltaNew = dot_lfvector(r, p, numverts);
1060
1061         delta0 = deltaNew * sqrt(conjgrad_epsilon);
1062 #endif
1063
1064 #ifdef DEBUG_TIME
1065         double start = PIL_check_seconds_timer();
1066 #endif
1067
1068         tol = (0.01*0.2);
1069
1070         while ((deltaNew > delta0*tol*tol) && (iterations < conjgrad_looplimit))
1071         {
1072                 iterations++;
1073
1074                 mul_bfmatrix_lfvector(s, lA, p);
1075                 filter(s, S);
1076
1077                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1078
1079                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1080
1081                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1082
1083                 mul_prevfmatrix_lfvector(h, Pinv, r);
1084                 filter(h, S);
1085
1086                 deltaOld = deltaNew;
1087
1088                 deltaNew = dot_lfvector(r, h, numverts);
1089
1090                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1091
1092                 filter(p, S);
1093
1094         }
1095
1096 #ifdef DEBUG_TIME
1097         double end = PIL_check_seconds_timer();
1098         printf("cg_filtered_pre time: %f\n", (float)(end - start));
1099 #endif
1100
1101         del_lfvector(btemp);
1102         del_lfvector(bhat);
1103         del_lfvector(h);
1104         del_lfvector(s);
1105         del_lfvector(p);
1106         del_lfvector(r);
1107
1108         // printf("iterations: %d\n", iterations);
1109
1110         return iterations<conjgrad_looplimit;
1111 }
1112 #endif
1113
1114 bool BPH_mass_spring_solve_velocities(Implicit_Data *data, float dt, ImplicitSolverResult *result)
1115 {
1116         unsigned int numverts = data->dFdV[0].vcount;
1117
1118         lfVector *dFdXmV = create_lfvector(numverts);
1119         zero_lfvector(data->dV, numverts);
1120
1121         cp_bfmatrix(data->A, data->M);
1122
1123         subadd_bfmatrixS_bfmatrixS(data->A, data->dFdV, dt, data->dFdX, (dt*dt));
1124
1125         mul_bfmatrix_lfvector(dFdXmV, data->dFdX, data->V);
1126
1127         add_lfvectorS_lfvectorS(data->B, data->F, dt, dFdXmV, (dt*dt), numverts);
1128
1129 #ifdef DEBUG_TIME
1130         double start = PIL_check_seconds_timer();
1131 #endif
1132
1133         cg_filtered(data->dV, data->A, data->B, data->z, data->S, result); /* conjugate gradient algorithm to solve Ax=b */
1134         // cg_filtered_pre(id->dV, id->A, id->B, id->z, id->S, id->P, id->Pinv, id->bigI);
1135
1136 #ifdef DEBUG_TIME
1137         double end = PIL_check_seconds_timer();
1138         printf("cg_filtered calc time: %f\n", (float)(end - start));
1139 #endif
1140
1141         // advance velocities
1142         add_lfvector_lfvector(data->Vnew, data->V, data->dV, numverts);
1143
1144         del_lfvector(dFdXmV);
1145
1146         return result->status == BPH_SOLVER_SUCCESS;
1147 }
1148
1149 bool BPH_mass_spring_solve_positions(Implicit_Data *data, float dt)
1150 {
1151         int numverts = data->M[0].vcount;
1152
1153         // advance positions
1154         add_lfvector_lfvectorS(data->Xnew, data->X, data->Vnew, dt, numverts);
1155
1156         return true;
1157 }
1158
1159 void BPH_mass_spring_apply_result(Implicit_Data *data)
1160 {
1161         int numverts = data->M[0].vcount;
1162         cp_lfvector(data->X, data->Xnew, numverts);
1163         cp_lfvector(data->V, data->Vnew, numverts);
1164 }
1165
1166 void BPH_mass_spring_set_vertex_mass(Implicit_Data *data, int index, float mass)
1167 {
1168         unit_m3(data->M[index].m);
1169         mul_m3_fl(data->M[index].m, mass);
1170 }
1171
1172 void BPH_mass_spring_set_rest_transform(Implicit_Data *data, int index, float tfm[3][3])
1173 {
1174 #ifdef CLOTH_ROOT_FRAME
1175         copy_m3_m3(data->tfm[index].m, tfm);
1176 #else
1177         unit_m3(data->tfm[index].m);
1178         (void)tfm;
1179 #endif
1180 }
1181
1182 void BPH_mass_spring_set_motion_state(Implicit_Data *data, int index, const float x[3], const float v[3])
1183 {
1184         world_to_root_v3(data, index, data->X[index], x);
1185         world_to_root_v3(data, index, data->V[index], v);
1186 }
1187
1188 void BPH_mass_spring_set_position(Implicit_Data *data, int index, const float x[3])
1189 {
1190         world_to_root_v3(data, index, data->X[index], x);
1191 }
1192
1193 void BPH_mass_spring_set_velocity(Implicit_Data *data, int index, const float v[3])
1194 {
1195         world_to_root_v3(data, index, data->V[index], v);
1196 }
1197
1198 void BPH_mass_spring_get_motion_state(struct Implicit_Data *data, int index, float x[3], float v[3])
1199 {
1200         if (x) root_to_world_v3(data, index, x, data->X[index]);
1201         if (v) root_to_world_v3(data, index, v, data->V[index]);
1202 }
1203
1204 void BPH_mass_spring_get_position(struct Implicit_Data *data, int index, float x[3])
1205 {
1206         root_to_world_v3(data, index, x, data->X[index]);
1207 }
1208
1209 void BPH_mass_spring_get_new_position(struct Implicit_Data *data, int index, float x[3])
1210 {
1211         root_to_world_v3(data, index, x, data->Xnew[index]);
1212 }
1213
1214 void BPH_mass_spring_set_new_position(struct Implicit_Data *data, int index, const float x[3])
1215 {
1216         world_to_root_v3(data, index, data->Xnew[index], x);
1217 }
1218
1219 void BPH_mass_spring_get_new_velocity(struct Implicit_Data *data, int index, float v[3])
1220 {
1221         root_to_world_v3(data, index, v, data->Vnew[index]);
1222 }
1223
1224 void BPH_mass_spring_set_new_velocity(struct Implicit_Data *data, int index, const float v[3])
1225 {
1226         world_to_root_v3(data, index, data->Vnew[index], v);
1227 }
1228
1229 /* -------------------------------- */
1230
1231 static int BPH_mass_spring_add_block(Implicit_Data *data, int v1, int v2)
1232 {
1233         int s = data->M[0].vcount + data->num_blocks; /* index from array start */
1234         BLI_assert(s < data->M[0].vcount + data->M[0].scount);
1235         ++data->num_blocks;
1236
1237         /* tfm and S don't have spring entries (diagonal blocks only) */
1238         init_fmatrix(data->bigI + s, v1, v2);
1239         init_fmatrix(data->M + s, v1, v2);
1240         init_fmatrix(data->dFdX + s, v1, v2);
1241         init_fmatrix(data->dFdV + s, v1, v2);
1242         init_fmatrix(data->A + s, v1, v2);
1243         init_fmatrix(data->P + s, v1, v2);
1244         init_fmatrix(data->Pinv + s, v1, v2);
1245
1246         return s;
1247 }
1248
1249 void BPH_mass_spring_clear_constraints(Implicit_Data *data)
1250 {
1251         int i, numverts = data->S[0].vcount;
1252         for (i = 0; i < numverts; ++i) {
1253                 unit_m3(data->S[i].m);
1254                 zero_v3(data->z[i]);
1255         }
1256 }
1257
1258 void BPH_mass_spring_add_constraint_ndof0(Implicit_Data *data, int index, const float dV[3])
1259 {
1260         zero_m3(data->S[index].m);
1261
1262         world_to_root_v3(data, index, data->z[index], dV);
1263 }
1264
1265 void BPH_mass_spring_add_constraint_ndof1(Implicit_Data *data, int index, const float c1[3], const float c2[3], const float dV[3])
1266 {
1267         float m[3][3], p[3], q[3], u[3], cmat[3][3];
1268
1269         world_to_root_v3(data, index, p, c1);
1270         mul_fvectorT_fvector(cmat, p, p);
1271         sub_m3_m3m3(m, I, cmat);
1272
1273         world_to_root_v3(data, index, q, c2);
1274         mul_fvectorT_fvector(cmat, q, q);
1275         sub_m3_m3m3(m, m, cmat);
1276
1277         /* XXX not sure but multiplication should work here */
1278         copy_m3_m3(data->S[index].m, m);
1279 //      mul_m3_m3m3(data->S[index].m, data->S[index].m, m);
1280
1281         world_to_root_v3(data, index, u, dV);
1282         add_v3_v3(data->z[index], u);
1283 }
1284
1285 void BPH_mass_spring_add_constraint_ndof2(Implicit_Data *data, int index, const float c1[3], const float dV[3])
1286 {
1287         float m[3][3], p[3], u[3], cmat[3][3];
1288
1289         world_to_root_v3(data, index, p, c1);
1290         mul_fvectorT_fvector(cmat, p, p);
1291         sub_m3_m3m3(m, I, cmat);
1292
1293         copy_m3_m3(data->S[index].m, m);
1294 //      mul_m3_m3m3(data->S[index].m, data->S[index].m, m);
1295
1296         world_to_root_v3(data, index, u, dV);
1297         add_v3_v3(data->z[index], u);
1298 }
1299
1300 void BPH_mass_spring_clear_forces(Implicit_Data *data)
1301 {
1302         int numverts = data->M[0].vcount;
1303         zero_lfvector(data->F, numverts);
1304         init_bfmatrix(data->dFdX, ZERO);
1305         init_bfmatrix(data->dFdV, ZERO);
1306
1307         data->num_blocks = 0;
1308 }
1309
1310 void BPH_mass_spring_force_reference_frame(Implicit_Data *data, int index, const float acceleration[3], const float omega[3], const float domega_dt[3], float mass)
1311 {
1312 #ifdef CLOTH_ROOT_FRAME
1313         float acc[3], w[3], dwdt[3];
1314         float f[3], dfdx[3][3], dfdv[3][3];
1315         float euler[3], coriolis[3], centrifugal[3], rotvel[3];
1316         float deuler[3][3], dcoriolis[3][3], dcentrifugal[3][3], drotvel[3][3];
1317
1318         world_to_root_v3(data, index, acc, acceleration);
1319         world_to_root_v3(data, index, w, omega);
1320         world_to_root_v3(data, index, dwdt, domega_dt);
1321
1322         cross_v3_v3v3(euler, dwdt, data->X[index]);
1323         cross_v3_v3v3(coriolis, w, data->V[index]);
1324         mul_v3_fl(coriolis, 2.0f);
1325         cross_v3_v3v3(rotvel, w, data->X[index]);
1326         cross_v3_v3v3(centrifugal, w, rotvel);
1327
1328         sub_v3_v3v3(f, acc, euler);
1329         sub_v3_v3(f, coriolis);
1330         sub_v3_v3(f, centrifugal);
1331
1332         mul_v3_fl(f, mass); /* F = m * a */
1333
1334         cross_v3_identity(deuler, dwdt);
1335         cross_v3_identity(dcoriolis, w);
1336         mul_m3_fl(dcoriolis, 2.0f);
1337         cross_v3_identity(drotvel, w);
1338         cross_m3_v3m3(dcentrifugal, w, drotvel);
1339
1340         add_m3_m3m3(dfdx, deuler, dcentrifugal);
1341         negate_m3(dfdx);
1342         mul_m3_fl(dfdx, mass);
1343
1344         copy_m3_m3(dfdv, dcoriolis);
1345         negate_m3(dfdv);
1346         mul_m3_fl(dfdv, mass);
1347
1348         add_v3_v3(data->F[index], f);
1349         add_m3_m3m3(data->dFdX[index].m, data->dFdX[index].m, dfdx);
1350         add_m3_m3m3(data->dFdV[index].m, data->dFdV[index].m, dfdv);
1351 #else
1352         (void)data;
1353         (void)index;
1354         (void)acceleration;
1355         (void)omega;
1356         (void)domega_dt;
1357 #endif
1358 }
1359
1360 void BPH_mass_spring_force_gravity(Implicit_Data *data, int index, float mass, const float g[3])
1361 {
1362         /* force = mass * acceleration (in this case: gravity) */
1363         float f[3];
1364         world_to_root_v3(data, index, f, g);
1365         mul_v3_fl(f, mass);
1366
1367         add_v3_v3(data->F[index], f);
1368 }
1369
1370 void BPH_mass_spring_force_drag(Implicit_Data *data, float drag)
1371 {
1372         int i, numverts = data->M[0].vcount;
1373         for (i = 0; i < numverts; i++) {
1374                 float tmp[3][3];
1375
1376                 /* NB: uses root space velocity, no need to transform */
1377                 madd_v3_v3fl(data->F[i], data->V[i], -drag);
1378
1379                 copy_m3_m3(tmp, I);
1380                 mul_m3_fl(tmp, -drag);
1381                 add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, tmp);
1382         }
1383 }
1384
1385 void BPH_mass_spring_force_extern(struct Implicit_Data *data, int i, const float f[3], float dfdx[3][3], float dfdv[3][3])
1386 {
1387         float tf[3], tdfdx[3][3], tdfdv[3][3];
1388         world_to_root_v3(data, i, tf, f);
1389         world_to_root_m3(data, i, tdfdx, dfdx);
1390         world_to_root_m3(data, i, tdfdv, dfdv);
1391
1392         add_v3_v3(data->F[i], tf);
1393         add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, tdfdx);
1394         add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, tdfdv);
1395 }
1396
1397 static float calc_nor_area_tri(float nor[3], const float v1[3], const float v2[3], const float v3[3])
1398 {
1399         float n1[3], n2[3];
1400
1401         sub_v3_v3v3(n1, v1, v2);
1402         sub_v3_v3v3(n2, v2, v3);
1403
1404         cross_v3_v3v3(nor, n1, n2);
1405         return normalize_v3(nor);
1406 }
1407
1408 /* XXX does not support force jacobians yet, since the effector system does not provide them either */
1409 void BPH_mass_spring_force_face_wind(Implicit_Data *data, int v1, int v2, int v3, const float (*winvec)[3])
1410 {
1411         const float effector_scale = 0.02f;
1412         float win[3], nor[3], area;
1413         float factor;
1414
1415         /* calculate face normal and area */
1416         area = calc_nor_area_tri(nor, data->X[v1], data->X[v2], data->X[v3]);
1417         factor = effector_scale * area / 3.0f;
1418
1419         world_to_root_v3(data, v1, win, winvec[v1]);
1420         madd_v3_v3fl(data->F[v1], nor, factor * dot_v3v3(win, nor));
1421
1422         world_to_root_v3(data, v2, win, winvec[v2]);
1423         madd_v3_v3fl(data->F[v2], nor, factor * dot_v3v3(win, nor));
1424
1425         world_to_root_v3(data, v3, win, winvec[v3]);
1426         madd_v3_v3fl(data->F[v3], nor, factor * dot_v3v3(win, nor));
1427 }
1428
1429 static void edge_wind_vertex(const float dir[3], float length, float radius, const float wind[3], float f[3], float UNUSED(dfdx[3][3]), float UNUSED(dfdv[3][3]))
1430 {
1431         const float density = 0.01f; /* XXX arbitrary value, corresponds to effect of air density */
1432         float cos_alpha, sin_alpha, cross_section;
1433         float windlen = len_v3(wind);
1434
1435         if (windlen == 0.0f) {
1436                 zero_v3(f);
1437                 return;
1438         }
1439
1440         /* angle of wind direction to edge */
1441         cos_alpha = dot_v3v3(wind, dir) / windlen;
1442         sin_alpha = sqrtf(1.0f - cos_alpha * cos_alpha);
1443         cross_section = radius * ((float)M_PI * radius * sin_alpha + length * cos_alpha);
1444
1445         mul_v3_v3fl(f, wind, density * cross_section);
1446 }
1447
1448 void BPH_mass_spring_force_edge_wind(Implicit_Data *data, int v1, int v2, float radius1, float radius2, const float (*winvec)[3])
1449 {
1450         float win[3], dir[3], length;
1451         float f[3], dfdx[3][3], dfdv[3][3];
1452
1453         sub_v3_v3v3(dir, data->X[v1], data->X[v2]);
1454         length = normalize_v3(dir);
1455
1456         world_to_root_v3(data, v1, win, winvec[v1]);
1457         edge_wind_vertex(dir, length, radius1, win, f, dfdx, dfdv);
1458         add_v3_v3(data->F[v1], f);
1459
1460         world_to_root_v3(data, v2, win, winvec[v2]);
1461         edge_wind_vertex(dir, length, radius2, win, f, dfdx, dfdv);
1462         add_v3_v3(data->F[v2], f);
1463 }
1464
1465 void BPH_mass_spring_force_vertex_wind(Implicit_Data *data, int v, float UNUSED(radius), const float (*winvec)[3])
1466 {
1467         const float density = 0.01f; /* XXX arbitrary value, corresponds to effect of air density */
1468
1469         float wind[3];
1470         float f[3];
1471
1472         world_to_root_v3(data, v, wind, winvec[v]);
1473         mul_v3_v3fl(f, wind, density);
1474         add_v3_v3(data->F[v], f);
1475 }
1476
1477 BLI_INLINE void dfdx_spring(float to[3][3], const float dir[3], float length, float L, float k)
1478 {
1479         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1480         //return  ( (I-outerprod(dir, dir))*Min(1.0f, rest/length) - I) * -k;
1481         outerproduct(to, dir, dir);
1482         sub_m3_m3m3(to, I, to);
1483
1484         mul_m3_fl(to, (L/length));
1485         sub_m3_m3m3(to, to, I);
1486         mul_m3_fl(to, k);
1487 }
1488
1489 /* unused */
1490 #if 0
1491 BLI_INLINE void dfdx_damp(float to[3][3], const float dir[3], float length, const float vel[3], float rest, float damping)
1492 {
1493         // inner spring damping   vel is the relative velocity  of the endpoints.
1494         //      return (I-outerprod(dir, dir)) * (-damping * -(dot(dir, vel)/Max(length, rest)));
1495         mul_fvectorT_fvector(to, dir, dir);
1496         sub_fmatrix_fmatrix(to, I, to);
1497         mul_fmatrix_S(to,  (-damping * -(dot_v3v3(dir, vel)/MAX2(length, rest))));
1498 }
1499 #endif
1500
1501 BLI_INLINE void dfdv_damp(float to[3][3], const float dir[3], float damping)
1502 {
1503         // derivative of force wrt velocity
1504         outerproduct(to, dir, dir);
1505         mul_m3_fl(to, -damping);
1506 }
1507
1508 BLI_INLINE float fb(float length, float L)
1509 {
1510         float x = length / L;
1511         float xx = x * x;
1512         float xxx = xx * x;
1513         float xxxx = xxx * x;
1514         return (-11.541f * xxxx + 34.193f * xxx - 39.083f * xx + 23.116f * x - 9.713f);
1515 }
1516
1517 BLI_INLINE float fbderiv(float length, float L)
1518 {
1519         float x = length/L;
1520         float xx = x * x;
1521         float xxx = xx * x;
1522         return (-46.164f * xxx + 102.579f * xx - 78.166f * x + 23.116f);
1523 }
1524
1525 BLI_INLINE float fbstar(float length, float L, float kb, float cb)
1526 {
1527         float tempfb_fl = kb * fb(length, L);
1528         float fbstar_fl = cb * (length - L);
1529
1530         if (tempfb_fl < fbstar_fl)
1531                 return fbstar_fl;
1532         else
1533                 return tempfb_fl;
1534 }
1535
1536 // function to calculae bending spring force (taken from Choi & Co)
1537 BLI_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
1538 {
1539         float tempfb_fl = kb * fb(length, L);
1540         float fbstar_fl = cb * (length - L);
1541
1542         if (tempfb_fl < fbstar_fl) {
1543                 return -cb;
1544         }
1545         else {
1546                 return -kb * fbderiv(length, L);
1547         }
1548 }
1549
1550 /* calculate elonglation */
1551 BLI_INLINE bool spring_length(Implicit_Data *data, int i, int j, float r_extent[3], float r_dir[3], float *r_length, float r_vel[3])
1552 {
1553         sub_v3_v3v3(r_extent, data->X[j], data->X[i]);
1554         sub_v3_v3v3(r_vel, data->V[j], data->V[i]);
1555         *r_length = len_v3(r_extent);
1556
1557         if (*r_length > ALMOST_ZERO) {
1558 #if 0
1559                 if (length > L) {
1560                         if ((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) &&
1561                             ( ((length-L)*100.0f/L) > clmd->sim_parms->maxspringlen ))
1562                         {
1563                                 // cut spring!
1564                                 s->flags |= CSPRING_FLAG_DEACTIVATE;
1565                                 return false;
1566                         }
1567                 }
1568 #endif
1569                 mul_v3_v3fl(r_dir, r_extent, 1.0f/(*r_length));
1570         }
1571         else {
1572                 zero_v3(r_dir);
1573         }
1574
1575         return true;
1576 }
1577
1578 BLI_INLINE void apply_spring(Implicit_Data *data, int i, int j, const float f[3], float dfdx[3][3], float dfdv[3][3])
1579 {
1580         int block_ij = BPH_mass_spring_add_block(data, i, j);
1581
1582         add_v3_v3(data->F[i], f);
1583         sub_v3_v3(data->F[j], f);
1584
1585         add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfdx);
1586         add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfdx);
1587         sub_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfdx);
1588
1589         add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, dfdv);
1590         add_m3_m3m3(data->dFdV[j].m, data->dFdV[j].m, dfdv);
1591         sub_m3_m3m3(data->dFdV[block_ij].m, data->dFdV[block_ij].m, dfdv);
1592 }
1593
1594 bool BPH_mass_spring_force_spring_linear(Implicit_Data *data, int i, int j, float restlen,
1595                                          float stiffness_tension, float damping_tension,
1596                                          float stiffness_compression, float damping_compression,
1597                                          bool resist_compress, bool new_compress, float clamp_force)
1598 {
1599         float extent[3], length, dir[3], vel[3];
1600         float f[3], dfdx[3][3], dfdv[3][3];
1601         float damping = 0;
1602
1603         // calculate elonglation
1604         spring_length(data, i, j, extent, dir, &length, vel);
1605
1606         /* This code computes not only the force, but also its derivative.
1607            Zero derivative effectively disables the spring for the implicit solver.
1608            Thus length > restlen makes cloth unconstrained at the start of simulation. */
1609         if ((length >= restlen && length > 0) || resist_compress) {
1610                 float stretch_force;
1611
1612                 damping = damping_tension;
1613
1614                 stretch_force = stiffness_tension * (length - restlen);
1615                 if (clamp_force > 0.0f && stretch_force > clamp_force) {
1616                         stretch_force = clamp_force;
1617                 }
1618                 mul_v3_v3fl(f, dir, stretch_force);
1619
1620                 dfdx_spring(dfdx, dir, length, restlen, stiffness_tension);
1621         }
1622         else if (new_compress) {
1623                 /* This is based on the Choi and Ko bending model, which works surprisingly well for compression. */
1624                 float kb = stiffness_compression;
1625                 float cb = kb; /* cb equal to kb seems to work, but a factor can be added if necessary */
1626
1627                 damping = damping_compression;
1628
1629                 mul_v3_v3fl(f, dir, fbstar(length, restlen, kb, cb));
1630
1631                 outerproduct(dfdx, dir, dir);
1632                 mul_m3_fl(dfdx, fbstar_jacobi(length, restlen, kb, cb));
1633         }
1634         else {
1635                 return false;
1636         }
1637
1638         madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir));
1639         dfdv_damp(dfdv, dir, damping);
1640
1641         apply_spring(data, i, j, f, dfdx, dfdv);
1642
1643         return true;
1644 }
1645
1646 /* See "Stable but Responsive Cloth" (Choi, Ko 2005) */
1647 bool BPH_mass_spring_force_spring_bending(Implicit_Data *data, int i, int j, float restlen, float kb, float cb)
1648 {
1649         float extent[3], length, dir[3], vel[3];
1650
1651         // calculate elonglation
1652         spring_length(data, i, j, extent, dir, &length, vel);
1653
1654         if (length < restlen) {
1655                 float f[3], dfdx[3][3], dfdv[3][3];
1656
1657                 mul_v3_v3fl(f, dir, fbstar(length, restlen, kb, cb));
1658
1659                 outerproduct(dfdx, dir, dir);
1660                 mul_m3_fl(dfdx, fbstar_jacobi(length, restlen, kb, cb));
1661
1662                 /* XXX damping not supported */
1663                 zero_m3(dfdv);
1664
1665                 apply_spring(data, i, j, f, dfdx, dfdv);
1666
1667                 return true;
1668         }
1669         else {
1670                 return false;
1671         }
1672 }
1673
1674 BLI_INLINE void poly_avg(lfVector *data, int *inds, int len, float r_avg[3])
1675 {
1676         float fact = 1.0f / (float)len;
1677
1678         zero_v3(r_avg);
1679
1680         for (int i = 0; i < len; i++) {
1681                 madd_v3_v3fl(r_avg, data[inds[i]], fact);
1682         }
1683 }
1684
1685 BLI_INLINE void poly_norm(lfVector *data, int i, int j, int *inds, int len, float r_dir[3])
1686 {
1687         float mid[3];
1688
1689         poly_avg(data, inds, len, mid);
1690
1691         normal_tri_v3(r_dir, data[i], data[j], mid);
1692 }
1693
1694 BLI_INLINE void edge_avg(lfVector *data, int i, int j, float r_avg[3])
1695 {
1696         r_avg[0] = (data[i][0] + data[j][0]) * 0.5f;
1697         r_avg[1] = (data[i][1] + data[j][1]) * 0.5f;
1698         r_avg[2] = (data[i][2] + data[j][2]) * 0.5f;
1699 }
1700
1701 BLI_INLINE void edge_norm(lfVector *data, int i, int j, float r_dir[3])
1702 {
1703         sub_v3_v3v3(r_dir, data[i], data[j]);
1704         normalize_v3(r_dir);
1705 }
1706
1707 BLI_INLINE float bend_angle(float dir_a[3], float dir_b[3], float dir_e[3])
1708 {
1709         float cos, sin;
1710         float tmp[3];
1711
1712         cos = dot_v3v3(dir_a, dir_b);
1713
1714         cross_v3_v3v3(tmp, dir_a, dir_b);
1715         sin = dot_v3v3(tmp, dir_e);
1716
1717         return atan2f(sin, cos);
1718 }
1719
1720 BLI_INLINE void spring_angle(Implicit_Data *data, int i, int j, int *i_a, int *i_b, int len_a, int len_b,
1721                              float r_dir_a[3], float r_dir_b[3],
1722                              float *r_angle, float r_vel_a[3], float r_vel_b[3])
1723 {
1724         float dir_e[3], vel_e[3];
1725
1726         poly_norm(data->X, j, i, i_a, len_a, r_dir_a);
1727         poly_norm(data->X, i, j, i_b, len_b, r_dir_b);
1728
1729         edge_norm(data->X, i, j, dir_e);
1730
1731         *r_angle = bend_angle(r_dir_a, r_dir_b, dir_e);
1732
1733         poly_avg(data->V, i_a, len_a, r_vel_a);
1734         poly_avg(data->V, i_b, len_b, r_vel_b);
1735
1736         edge_avg(data->V, i, j, vel_e);
1737
1738         sub_v3_v3(r_vel_a, vel_e);
1739         sub_v3_v3(r_vel_b, vel_e);
1740 }
1741
1742 /* Angular springs roughly based on the bending model proposed by Baraff and Witkin in "Large Steps in Cloth Simulation". */
1743 bool BPH_mass_spring_force_spring_angular(Implicit_Data *data, int i, int j, int *i_a, int *i_b, int len_a, int len_b,
1744                                           float restang, float stiffness, float damping)
1745 {
1746         float angle, dir_a[3], dir_b[3], vel_a[3], vel_b[3];
1747         float f_a[3], f_b[3], f_e[3];
1748         float force;
1749         int x;
1750
1751         spring_angle(data, i, j, i_a, i_b, len_a, len_b,
1752                      dir_a, dir_b, &angle, vel_a, vel_b);
1753
1754         /* spring force */
1755         force = stiffness * (angle - restang);
1756
1757         /* damping force */
1758         force += -damping * (dot_v3v3(vel_a, dir_a) + dot_v3v3(vel_b, dir_b));
1759
1760         mul_v3_v3fl(f_a, dir_a, force / len_a);
1761         mul_v3_v3fl(f_b, dir_b, force / len_b);
1762
1763         for (x = 0; x < len_a; x++) {
1764                 add_v3_v3(data->F[i_a[x]], f_a);
1765         }
1766
1767         for (x = 0; x < len_b; x++) {
1768                 add_v3_v3(data->F[i_b[x]], f_b);
1769         }
1770
1771         mul_v3_v3fl(f_a, dir_a, force * 0.5f);
1772         mul_v3_v3fl(f_b, dir_b, force * 0.5f);
1773
1774         add_v3_v3v3(f_e, f_a, f_b);
1775
1776         sub_v3_v3(data->F[i], f_e);
1777         sub_v3_v3(data->F[j], f_e);
1778
1779         return true;
1780 }
1781
1782 /* Jacobian of a direction vector.
1783  * Basically the part of the differential orthogonal to the direction,
1784  * inversely proportional to the length of the edge.
1785  *
1786  * dD_ij/dx_i = -dD_ij/dx_j = (D_ij * D_ij^T - I) / len_ij
1787  */
1788 BLI_INLINE void spring_grad_dir(Implicit_Data *data, int i, int j, float edge[3], float dir[3], float grad_dir[3][3])
1789 {
1790         float length;
1791
1792         sub_v3_v3v3(edge, data->X[j], data->X[i]);
1793         length = normalize_v3_v3(dir, edge);
1794
1795         if (length > ALMOST_ZERO) {
1796                 outerproduct(grad_dir, dir, dir);
1797                 sub_m3_m3m3(grad_dir, I, grad_dir);
1798                 mul_m3_fl(grad_dir, 1.0f / length);
1799         }
1800         else {
1801                 zero_m3(grad_dir);
1802         }
1803 }
1804
1805 BLI_INLINE void spring_hairbend_forces(Implicit_Data *data, int i, int j, int k,
1806                                       const float goal[3],
1807                                       float stiffness, float damping,
1808                                       int q, const float dx[3], const float dv[3],
1809                                       float r_f[3])
1810 {
1811         float edge_ij[3], dir_ij[3];
1812         float edge_jk[3], dir_jk[3];
1813         float vel_ij[3], vel_jk[3], vel_ortho[3];
1814         float f_bend[3], f_damp[3];
1815         float fk[3];
1816         float dist[3];
1817
1818         zero_v3(fk);
1819
1820         sub_v3_v3v3(edge_ij, data->X[j], data->X[i]);
1821         if (q == i) sub_v3_v3(edge_ij, dx);
1822         if (q == j) add_v3_v3(edge_ij, dx);
1823         normalize_v3_v3(dir_ij, edge_ij);
1824
1825         sub_v3_v3v3(edge_jk, data->X[k], data->X[j]);
1826         if (q == j) sub_v3_v3(edge_jk, dx);
1827         if (q == k) add_v3_v3(edge_jk, dx);
1828         normalize_v3_v3(dir_jk, edge_jk);
1829
1830         sub_v3_v3v3(vel_ij, data->V[j], data->V[i]);
1831         if (q == i) sub_v3_v3(vel_ij, dv);
1832         if (q == j) add_v3_v3(vel_ij, dv);
1833
1834         sub_v3_v3v3(vel_jk, data->V[k], data->V[j]);
1835         if (q == j) sub_v3_v3(vel_jk, dv);
1836         if (q == k) add_v3_v3(vel_jk, dv);
1837
1838         /* bending force */
1839         sub_v3_v3v3(dist, goal, edge_jk);
1840         mul_v3_v3fl(f_bend, dist, stiffness);
1841
1842         add_v3_v3(fk, f_bend);
1843
1844         /* damping force */
1845         madd_v3_v3v3fl(vel_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk));
1846         mul_v3_v3fl(f_damp, vel_ortho, damping);
1847
1848         sub_v3_v3(fk, f_damp);
1849
1850         copy_v3_v3(r_f, fk);
1851 }
1852
1853 /* Finite Differences method for estimating the jacobian of the force */
1854 BLI_INLINE void spring_hairbend_estimate_dfdx(Implicit_Data *data, int i, int j, int k,
1855                                              const float goal[3],
1856                                              float stiffness, float damping,
1857                                              int q, float dfdx[3][3])
1858 {
1859         const float delta = 0.00001f; // TODO find a good heuristic for this
1860         float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3];
1861         float f[3];
1862         int a, b;
1863
1864         zero_m3(dvec_null);
1865         unit_m3(dvec_pos);
1866         mul_m3_fl(dvec_pos, delta * 0.5f);
1867         copy_m3_m3(dvec_neg, dvec_pos);
1868         negate_m3(dvec_neg);
1869
1870         /* XXX TODO offset targets to account for position dependency */
1871
1872         for (a = 0; a < 3; ++a) {
1873                 spring_hairbend_forces(data, i, j, k, goal, stiffness, damping,
1874                                       q, dvec_pos[a], dvec_null[a], f);
1875                 copy_v3_v3(dfdx[a], f);
1876
1877                 spring_hairbend_forces(data, i, j, k, goal, stiffness, damping,
1878                                       q, dvec_neg[a], dvec_null[a], f);
1879                 sub_v3_v3(dfdx[a], f);
1880
1881                 for (b = 0; b < 3; ++b) {
1882                         dfdx[a][b] /= delta;
1883                 }
1884         }
1885 }
1886
1887 /* Finite Differences method for estimating the jacobian of the force */
1888 BLI_INLINE void spring_hairbend_estimate_dfdv(Implicit_Data *data, int i, int j, int k,
1889                                              const float goal[3],
1890                                              float stiffness, float damping,
1891                                              int q, float dfdv[3][3])
1892 {
1893         const float delta = 0.00001f; // TODO find a good heuristic for this
1894         float dvec_null[3][3], dvec_pos[3][3], dvec_neg[3][3];
1895         float f[3];
1896         int a, b;
1897
1898         zero_m3(dvec_null);
1899         unit_m3(dvec_pos);
1900         mul_m3_fl(dvec_pos, delta * 0.5f);
1901         copy_m3_m3(dvec_neg, dvec_pos);
1902         negate_m3(dvec_neg);
1903
1904         /* XXX TODO offset targets to account for position dependency */
1905
1906         for (a = 0; a < 3; ++a) {
1907                 spring_hairbend_forces(data, i, j, k, goal, stiffness, damping,
1908                                       q, dvec_null[a], dvec_pos[a], f);
1909                 copy_v3_v3(dfdv[a], f);
1910
1911                 spring_hairbend_forces(data, i, j, k, goal, stiffness, damping,
1912                                       q, dvec_null[a], dvec_neg[a], f);
1913                 sub_v3_v3(dfdv[a], f);
1914
1915                 for (b = 0; b < 3; ++b) {
1916                         dfdv[a][b] /= delta;
1917                 }
1918         }
1919 }
1920
1921 /* Angular spring that pulls the vertex toward the local target
1922  * See "Artistic Simulation of Curly Hair" (Pixar technical memo #12-03a)
1923  */
1924 bool BPH_mass_spring_force_spring_bending_hair(Implicit_Data *data, int i, int j, int k,
1925                                                   const float target[3], float stiffness, float damping)
1926 {
1927         float goal[3];
1928         float fj[3], fk[3];
1929         float dfj_dxi[3][3], dfj_dxj[3][3], dfk_dxi[3][3], dfk_dxj[3][3], dfk_dxk[3][3];
1930         float dfj_dvi[3][3], dfj_dvj[3][3], dfk_dvi[3][3], dfk_dvj[3][3], dfk_dvk[3][3];
1931
1932         const float vecnull[3] = {0.0f, 0.0f, 0.0f};
1933
1934         int block_ij = BPH_mass_spring_add_block(data, i, j);
1935         int block_jk = BPH_mass_spring_add_block(data, j, k);
1936         int block_ik = BPH_mass_spring_add_block(data, i, k);
1937
1938         world_to_root_v3(data, j, goal, target);
1939
1940         spring_hairbend_forces(data, i, j, k, goal, stiffness, damping, k, vecnull, vecnull, fk);
1941         negate_v3_v3(fj, fk); /* counterforce */
1942
1943         spring_hairbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, i, dfk_dxi);
1944         spring_hairbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, j, dfk_dxj);
1945         spring_hairbend_estimate_dfdx(data, i, j, k, goal, stiffness, damping, k, dfk_dxk);
1946         copy_m3_m3(dfj_dxi, dfk_dxi); negate_m3(dfj_dxi);
1947         copy_m3_m3(dfj_dxj, dfk_dxj); negate_m3(dfj_dxj);
1948
1949         spring_hairbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, i, dfk_dvi);
1950         spring_hairbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, j, dfk_dvj);
1951         spring_hairbend_estimate_dfdv(data, i, j, k, goal, stiffness, damping, k, dfk_dvk);
1952         copy_m3_m3(dfj_dvi, dfk_dvi); negate_m3(dfj_dvi);
1953         copy_m3_m3(dfj_dvj, dfk_dvj); negate_m3(dfj_dvj);
1954
1955         /* add forces and jacobians to the solver data */
1956
1957         add_v3_v3(data->F[j], fj);
1958         add_v3_v3(data->F[k], fk);
1959
1960         add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfj_dxj);
1961         add_m3_m3m3(data->dFdX[k].m, data->dFdX[k].m, dfk_dxk);
1962
1963         add_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfj_dxi);
1964         add_m3_m3m3(data->dFdX[block_jk].m, data->dFdX[block_jk].m, dfk_dxj);
1965         add_m3_m3m3(data->dFdX[block_ik].m, data->dFdX[block_ik].m, dfk_dxi);
1966
1967         add_m3_m3m3(data->dFdV[j].m, data->dFdV[j].m, dfj_dvj);
1968         add_m3_m3m3(data->dFdV[k].m, data->dFdV[k].m, dfk_dvk);
1969
1970         add_m3_m3m3(data->dFdV[block_ij].m, data->dFdV[block_ij].m, dfj_dvi);
1971         add_m3_m3m3(data->dFdV[block_jk].m, data->dFdV[block_jk].m, dfk_dvj);
1972         add_m3_m3m3(data->dFdV[block_ik].m, data->dFdV[block_ik].m, dfk_dvi);
1973
1974
1975         /* XXX analytical calculation of derivatives below is incorrect.
1976          * This proved to be difficult, but for now just using the finite difference method for
1977          * estimating the jacobians should be sufficient.
1978          */
1979 #if 0
1980         float edge_ij[3], dir_ij[3], grad_dir_ij[3][3];
1981         float edge_jk[3], dir_jk[3], grad_dir_jk[3][3];
1982         float dist[3], vel_jk[3], vel_jk_ortho[3], projvel[3];
1983         float target[3];
1984         float tmp[3][3];
1985         float fi[3], fj[3], fk[3];
1986         float dfi_dxi[3][3], dfj_dxi[3][3], dfj_dxj[3][3], dfk_dxi[3][3], dfk_dxj[3][3], dfk_dxk[3][3];
1987         float dfdvi[3][3];
1988
1989         // TESTING
1990         damping = 0.0f;
1991
1992         zero_v3(fi);
1993         zero_v3(fj);
1994         zero_v3(fk);
1995         zero_m3(dfi_dxi);
1996         zero_m3(dfj_dxi);
1997         zero_m3(dfk_dxi);
1998         zero_m3(dfk_dxj);
1999         zero_m3(dfk_dxk);
2000
2001         /* jacobian of direction vectors */
2002         spring_grad_dir(data, i, j, edge_ij, dir_ij, grad_dir_ij);
2003         spring_grad_dir(data, j, k, edge_jk, dir_jk, grad_dir_jk);
2004
2005         sub_v3_v3v3(vel_jk, data->V[k], data->V[j]);
2006
2007         /* bending force */
2008         mul_v3_v3fl(target, dir_ij, restlen);
2009         sub_v3_v3v3(dist, target, edge_jk);
2010         mul_v3_v3fl(fk, dist, stiffness);
2011
2012         /* damping force */
2013         madd_v3_v3v3fl(vel_jk_ortho, vel_jk, dir_jk, -dot_v3v3(vel_jk, dir_jk));
2014         madd_v3_v3fl(fk, vel_jk_ortho, damping);
2015
2016         /* XXX this only holds true as long as we assume straight rest shape!
2017          * eventually will become a bit more involved since the opposite segment
2018          * gets its own target, under condition of having equal torque on both sides.
2019          */
2020         copy_v3_v3(fi, fk);
2021
2022         /* counterforce on the middle point */
2023         sub_v3_v3(fj, fi);
2024         sub_v3_v3(fj, fk);
2025
2026         /* === derivatives === */
2027
2028         madd_m3_m3fl(dfk_dxi, grad_dir_ij, stiffness * restlen);
2029
2030         madd_m3_m3fl(dfk_dxj, grad_dir_ij, -stiffness * restlen);
2031         madd_m3_m3fl(dfk_dxj, I, stiffness);
2032
2033         madd_m3_m3fl(dfk_dxk, I, -stiffness);
2034
2035         copy_m3_m3(dfi_dxi, dfk_dxk);
2036         negate_m3(dfi_dxi);
2037
2038         /* dfj_dfi == dfi_dfj due to symmetry,
2039          * dfi_dfj == dfk_dfj due to fi == fk
2040          * XXX see comment above on future bent rest shapes
2041          */
2042         copy_m3_m3(dfj_dxi, dfk_dxj);
2043
2044         /* dfj_dxj == -(dfi_dxj + dfk_dxj) due to fj == -(fi + fk) */
2045         sub_m3_m3m3(dfj_dxj, dfj_dxj, dfj_dxi);
2046         sub_m3_m3m3(dfj_dxj, dfj_dxj, dfk_dxj);
2047
2048         /* add forces and jacobians to the solver data */
2049         add_v3_v3(data->F[i], fi);
2050         add_v3_v3(data->F[j], fj);
2051         add_v3_v3(data->F[k], fk);
2052
2053         add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfi_dxi);
2054         add_m3_m3m3(data->dFdX[j].m, data->dFdX[j].m, dfj_dxj);
2055         add_m3_m3m3(data->dFdX[k].m, data->dFdX[k].m, dfk_dxk);
2056
2057         add_m3_m3m3(data->dFdX[block_ij].m, data->dFdX[block_ij].m, dfj_dxi);
2058         add_m3_m3m3(data->dFdX[block_jk].m, data->dFdX[block_jk].m, dfk_dxj);
2059         add_m3_m3m3(data->dFdX[block_ik].m, data->dFdX[block_ik].m, dfk_dxi);
2060 #endif
2061
2062         return true;
2063 }
2064
2065 bool BPH_mass_spring_force_spring_goal(Implicit_Data *data, int i, const float goal_x[3], const float goal_v[3],
2066                                        float stiffness, float damping)
2067 {
2068         float root_goal_x[3], root_goal_v[3], extent[3], length, dir[3], vel[3];
2069         float f[3], dfdx[3][3], dfdv[3][3];
2070
2071         /* goal is in world space */
2072         world_to_root_v3(data, i, root_goal_x, goal_x);
2073         world_to_root_v3(data, i, root_goal_v, goal_v);
2074
2075         sub_v3_v3v3(extent, root_goal_x, data->X[i]);
2076         sub_v3_v3v3(vel, root_goal_v, data->V[i]);
2077         length = normalize_v3_v3(dir, extent);
2078
2079         if (length > ALMOST_ZERO) {
2080                 mul_v3_v3fl(f, dir, stiffness * length);
2081
2082                 // Ascher & Boxman, p.21: Damping only during elonglation
2083                 // something wrong with it...
2084                 madd_v3_v3fl(f, dir, damping * dot_v3v3(vel, dir));
2085
2086                 dfdx_spring(dfdx, dir, length, 0.0f, stiffness);
2087                 dfdv_damp(dfdv, dir, damping);
2088
2089                 add_v3_v3(data->F[i], f);
2090                 add_m3_m3m3(data->dFdX[i].m, data->dFdX[i].m, dfdx);
2091                 add_m3_m3m3(data->dFdV[i].m, data->dFdV[i].m, dfdv);
2092
2093                 return true;
2094         }
2095         else {
2096                 return false;
2097         }
2098 }
2099
2100 #endif /* IMPLICIT_SOLVER_BLENDER */