diff options
author | Lukas Tönne <lukas.toenne@gmail.com> | 2014-09-13 16:36:46 +0400 |
---|---|---|
committer | Lukas Tönne <lukas.toenne@gmail.com> | 2015-01-20 11:30:00 +0300 |
commit | 5322def57cdbb81d38c5ba8e8e617c697791d22b (patch) | |
tree | 9f3b042615975de2cc076dae9f6f2c8e9fc24f08 /source/blender/blenkernel/intern/implicit.c | |
parent | 1c8a33ab92ce8935165be29d2fc93e459c191286 (diff) |
Moved the cloth solver code into a new subfolder/library inside Blender
code.
The implicit solver itself should remain agnostic to the specifics of
the Blender data (cloth vs. hair). This way we could avoid the bloated
data conversion chain from particles/hair to derived mesh to cloth
modifier to implicit solver data and back. Every step in this chain adds
overhead as well as rounding errors and a possibility for bugs, not to
speak of making the code horribly complicated.
The new subfolder is named "physics" since it should be the start of a
somewhat "unified" physics systems combining all the various solvers in
the same place and managing things like synchronized time steps.
Diffstat (limited to 'source/blender/blenkernel/intern/implicit.c')
-rw-r--r-- | source/blender/blenkernel/intern/implicit.c | 2744 |
1 files changed, 0 insertions, 2744 deletions
diff --git a/source/blender/blenkernel/intern/implicit.c b/source/blender/blenkernel/intern/implicit.c deleted file mode 100644 index 9ae53293397..00000000000 --- a/source/blender/blenkernel/intern/implicit.c +++ /dev/null @@ -1,2744 +0,0 @@ -/* - * ***** BEGIN GPL LICENSE BLOCK ***** - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software Foundation, - * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. - * - * The Original Code is Copyright (C) Blender Foundation - * All rights reserved. - * - * The Original Code is: all of this file. - * - * Contributor(s): none yet. - * - * ***** END GPL LICENSE BLOCK ***** - */ - -/** \file blender/blenkernel/intern/implicit.c - * \ingroup bke - */ - -#include "implicit.h" - -#ifdef IMPLICIT_SOLVER_BLENDER - -#include "MEM_guardedalloc.h" - -#include "DNA_scene_types.h" -#include "DNA_object_types.h" -#include "DNA_object_force.h" -#include "DNA_meshdata_types.h" -#include "DNA_texture_types.h" - -#include "BLI_math.h" -#include "BLI_linklist.h" -#include "BLI_utildefines.h" - -#include "BKE_cloth.h" -#include "BKE_collision.h" -#include "BKE_effect.h" -#include "BKE_global.h" - -#ifdef __GNUC__ -# pragma GCC diagnostic ignored "-Wtype-limits" -#endif - -#ifdef _OPENMP -# define CLOTH_OPENMP_LIMIT 512 -#endif - -#if 0 /* debug timing */ -#ifdef _WIN32 -#include <windows.h> -static LARGE_INTEGER _itstart, _itend; -static LARGE_INTEGER ifreq; -static void itstart(void) -{ - static int first = 1; - if (first) { - QueryPerformanceFrequency(&ifreq); - first = 0; - } - QueryPerformanceCounter(&_itstart); -} -static void itend(void) -{ - QueryPerformanceCounter(&_itend); -} -double itval(void) -{ - return ((double)_itend.QuadPart - - (double)_itstart.QuadPart)/((double)ifreq.QuadPart); -} -#else -#include <sys/time.h> -// intrinsics need better compile flag checking -// #include <xmmintrin.h> -// #include <pmmintrin.h> -// #include <pthread.h> - -static struct timeval _itstart, _itend; -static struct timezone itz; -static void itstart(void) -{ - gettimeofday(&_itstart, &itz); -} -static void itend(void) -{ - gettimeofday(&_itend, &itz); -} -static double itval(void) -{ - double t1, t2; - t1 = (double)_itstart.tv_sec + (double)_itstart.tv_usec/(1000*1000); - t2 = (double)_itend.tv_sec + (double)_itend.tv_usec/(1000*1000); - return t2-t1; -} -#endif -#endif /* debug timing */ - -static float I[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; -static float ZERO[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - -/* ==== hash functions for debugging ==== */ -static unsigned int hash_int_2d(unsigned int kx, unsigned int ky) -{ -#define rot(x,k) (((x)<<(k)) | ((x)>>(32-(k)))) - - unsigned int a, b, c; - - a = b = c = 0xdeadbeef + (2 << 2) + 13; - a += kx; - b += ky; - - c ^= b; c -= rot(b,14); - a ^= c; a -= rot(c,11); - b ^= a; b -= rot(a,25); - c ^= b; c -= rot(b,16); - a ^= c; a -= rot(c,4); - b ^= a; b -= rot(a,14); - c ^= b; c -= rot(b,24); - - return c; - -#undef rot -} - -static int hash_vertex(int type, int vertex) -{ - return hash_int_2d((unsigned int)type, (unsigned int)vertex); -} - -static int hash_collpair(int type, CollPair *collpair) -{ - return hash_int_2d((unsigned int)type, hash_int_2d((unsigned int)collpair->face1, (unsigned int)collpair->face2)); -} -/* ================ */ - -/* -#define C99 -#ifdef C99 -#defineDO_INLINE inline -#else -#defineDO_INLINE static -#endif -*/ -struct Cloth; - -////////////////////////////////////////// -/* fast vector / matrix library, enhancements are welcome :) -dg */ -///////////////////////////////////////// - -/* DEFINITIONS */ -typedef float lfVector[3]; -typedef struct fmatrix3x3 { - float m[3][3]; /* 3x3 matrix */ - unsigned int c, r; /* column and row number */ - /* int pinned; // is this vertex allowed to move? */ - float n1, n2, n3; /* three normal vectors for collision constrains */ - unsigned int vcount; /* vertex count */ - unsigned int scount; /* spring count */ -} fmatrix3x3; - -/////////////////////////// -// float[3] vector -/////////////////////////// -/* simple vector code */ -/* STATUS: verified */ -DO_INLINE void mul_fvector_S(float to[3], float from[3], float scalar) -{ - to[0] = from[0] * scalar; - to[1] = from[1] * scalar; - to[2] = from[2] * scalar; -} -/* simple v^T * v product ("outer product") */ -/* STATUS: HAS TO BE verified (*should* work) */ -DO_INLINE void mul_fvectorT_fvector(float to[3][3], float vectorA[3], float vectorB[3]) -{ - mul_fvector_S(to[0], vectorB, vectorA[0]); - mul_fvector_S(to[1], vectorB, vectorA[1]); - mul_fvector_S(to[2], vectorB, vectorA[2]); -} -/* simple v^T * v product with scalar ("outer product") */ -/* STATUS: HAS TO BE verified (*should* work) */ -DO_INLINE void mul_fvectorT_fvectorS(float to[3][3], float vectorA[3], float vectorB[3], float aS) -{ - mul_fvectorT_fvector(to, vectorA, vectorB); - - mul_fvector_S(to[0], to[0], aS); - mul_fvector_S(to[1], to[1], aS); - mul_fvector_S(to[2], to[2], aS); -} - -#if 0 -/* printf vector[3] on console: for debug output */ -static void print_fvector(float m3[3]) -{ - printf("%f\n%f\n%f\n\n", m3[0], m3[1], m3[2]); -} - -/////////////////////////// -// long float vector float (*)[3] -/////////////////////////// -/* print long vector on console: for debug output */ -DO_INLINE void print_lfvector(float (*fLongVector)[3], unsigned int verts) -{ - unsigned int i = 0; - for (i = 0; i < verts; i++) { - print_fvector(fLongVector[i]); - } -} -#endif - -/* create long vector */ -DO_INLINE lfVector *create_lfvector(unsigned int verts) -{ - /* TODO: check if memory allocation was successful */ - return (lfVector *)MEM_callocN(verts * sizeof(lfVector), "cloth_implicit_alloc_vector"); - // return (lfVector *)cloth_aligned_malloc(&MEMORY_BASE, verts * sizeof(lfVector)); -} -/* delete long vector */ -DO_INLINE void del_lfvector(float (*fLongVector)[3]) -{ - if (fLongVector != NULL) { - MEM_freeN(fLongVector); - // cloth_aligned_free(&MEMORY_BASE, fLongVector); - } -} -/* copy long vector */ -DO_INLINE void cp_lfvector(float (*to)[3], float (*from)[3], unsigned int verts) -{ - memcpy(to, from, verts * sizeof(lfVector)); -} -/* init long vector with float[3] */ -DO_INLINE void init_lfvector(float (*fLongVector)[3], float vector[3], unsigned int verts) -{ - unsigned int i = 0; - for (i = 0; i < verts; i++) { - copy_v3_v3(fLongVector[i], vector); - } -} -/* zero long vector with float[3] */ -DO_INLINE void zero_lfvector(float (*to)[3], unsigned int verts) -{ - memset(to, 0.0f, verts * sizeof(lfVector)); -} -/* multiply long vector with scalar*/ -DO_INLINE void mul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts) -{ - unsigned int i = 0; - - for (i = 0; i < verts; i++) { - mul_fvector_S(to[i], fLongVector[i], scalar); - } -} -/* multiply long vector with scalar*/ -/* A -= B * float */ -DO_INLINE void submul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts) -{ - unsigned int i = 0; - for (i = 0; i < verts; i++) { - VECSUBMUL(to[i], fLongVector[i], scalar); - } -} -/* dot product for big vector */ -DO_INLINE float dot_lfvector(float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts) -{ - long i = 0; - float temp = 0.0; -// XXX brecht, disabled this for now (first schedule line was already disabled), -// due to non-commutative nature of floating point ops this makes the sim give -// different results each time you run it! -// schedule(guided, 2) -//#pragma omp parallel for reduction(+: temp) if (verts > CLOTH_OPENMP_LIMIT) - for (i = 0; i < (long)verts; i++) { - temp += dot_v3v3(fLongVectorA[i], fLongVectorB[i]); - } - return temp; -} -/* A = B + C --> for big vector */ -DO_INLINE void add_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts) -{ - unsigned int i = 0; - - for (i = 0; i < verts; i++) { - VECADD(to[i], fLongVectorA[i], fLongVectorB[i]); - } - -} -/* A = B + C * float --> for big vector */ -DO_INLINE void add_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts) -{ - unsigned int i = 0; - - for (i = 0; i < verts; i++) { - VECADDS(to[i], fLongVectorA[i], fLongVectorB[i], bS); - - } -} -/* A = B * float + C * float --> for big vector */ -DO_INLINE void add_lfvectorS_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float aS, float (*fLongVectorB)[3], float bS, unsigned int verts) -{ - unsigned int i = 0; - - for (i = 0; i < verts; i++) { - VECADDSS(to[i], fLongVectorA[i], aS, fLongVectorB[i], bS); - } -} -/* A = B - C * float --> for big vector */ -DO_INLINE void sub_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts) -{ - unsigned int i = 0; - for (i = 0; i < verts; i++) { - VECSUBS(to[i], fLongVectorA[i], fLongVectorB[i], bS); - } - -} -/* A = B - C --> for big vector */ -DO_INLINE void sub_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts) -{ - unsigned int i = 0; - - for (i = 0; i < verts; i++) { - sub_v3_v3v3(to[i], fLongVectorA[i], fLongVectorB[i]); - } - -} -/////////////////////////// -// 3x3 matrix -/////////////////////////// -#if 0 -/* printf 3x3 matrix on console: for debug output */ -static void print_fmatrix(float m3[3][3]) -{ - printf("%f\t%f\t%f\n", m3[0][0], m3[0][1], m3[0][2]); - printf("%f\t%f\t%f\n", m3[1][0], m3[1][1], m3[1][2]); - printf("%f\t%f\t%f\n\n", m3[2][0], m3[2][1], m3[2][2]); -} - -static void print_sparse_matrix(fmatrix3x3 *m) -{ - if (m) { - unsigned int i; - for (i = 0; i < m[0].vcount + m[0].scount; i++) { - printf("%d:\n", i); - print_fmatrix(m[i].m); - } - } -} -#endif - -static void print_lvector(lfVector *v, int numverts) -{ - int i; - for (i = 0; i < numverts; ++i) { - if (i > 0) - printf("\n"); - - printf("%f,\n", v[i][0]); - printf("%f,\n", v[i][1]); - printf("%f,\n", v[i][2]); - } -} - -static void print_bfmatrix(fmatrix3x3 *m) -{ - int tot = m[0].vcount + m[0].scount; - int size = m[0].vcount * 3; - float *t = MEM_callocN(sizeof(float) * size*size, "bfmatrix"); - int q, i, j; - - for (q = 0; q < tot; ++q) { - int k = 3 * m[q].r; - int l = 3 * m[q].c; - - for (j = 0; j < 3; ++j) { - for (i = 0; i < 3; ++i) { -// if (t[k + i + (l + j) * size] != 0.0f) { -// printf("warning: overwriting value at %d, %d\n", m[q].r, m[q].c); -// } - if (k == l) { - t[k + i + (k + j) * size] += m[q].m[i][j]; - } - else { - t[k + i + (l + j) * size] += m[q].m[i][j]; - t[l + j + (k + i) * size] += m[q].m[j][i]; - } - } - } - } - - for (j = 0; j < size; ++j) { - if (j > 0 && j % 3 == 0) - printf("\n"); - - for (i = 0; i < size; ++i) { - if (i > 0 && i % 3 == 0) - printf(" "); - - implicit_print_matrix_elem(t[i + j * size]); - } - printf("\n"); - } - - MEM_freeN(t); -} - -/* copy 3x3 matrix */ -DO_INLINE void cp_fmatrix(float to[3][3], float from[3][3]) -{ - // memcpy(to, from, sizeof (float) * 9); - copy_v3_v3(to[0], from[0]); - copy_v3_v3(to[1], from[1]); - copy_v3_v3(to[2], from[2]); -} - -/* copy 3x3 matrix */ -DO_INLINE void initdiag_fmatrixS(float to[3][3], float aS) -{ - cp_fmatrix(to, ZERO); - - to[0][0] = aS; - to[1][1] = aS; - to[2][2] = aS; -} - -#if 0 -/* calculate determinant of 3x3 matrix */ -DO_INLINE float det_fmatrix(float m[3][3]) -{ - 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] - -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]; -} - -DO_INLINE void inverse_fmatrix(float to[3][3], float from[3][3]) -{ - unsigned int i, j; - float d; - - if ((d=det_fmatrix(from)) == 0) { - printf("can't build inverse"); - exit(0); - } - for (i=0;i<3;i++) { - for (j=0;j<3;j++) { - int i1=(i+1)%3; - int i2=(i+2)%3; - int j1=(j+1)%3; - int j2=(j+2)%3; - // reverse indexs i&j to take transpose - to[j][i] = (from[i1][j1]*from[i2][j2]-from[i1][j2]*from[i2][j1])/d; - /* - if (i==j) - to[i][j] = 1.0f / from[i][j]; - else - to[i][j] = 0; - */ - } - } - -} -#endif - -/* 3x3 matrix multiplied by a scalar */ -/* STATUS: verified */ -DO_INLINE void mul_fmatrix_S(float matrix[3][3], float scalar) -{ - mul_fvector_S(matrix[0], matrix[0], scalar); - mul_fvector_S(matrix[1], matrix[1], scalar); - mul_fvector_S(matrix[2], matrix[2], scalar); -} - -/* a vector multiplied by a 3x3 matrix */ -/* STATUS: verified */ -DO_INLINE void mul_fvector_fmatrix(float *to, float *from, float matrix[3][3]) -{ - to[0] = matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2]; - to[1] = matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2]; - to[2] = matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2]; -} - -/* 3x3 matrix multiplied by a vector */ -/* STATUS: verified */ -DO_INLINE void mul_fmatrix_fvector(float *to, float matrix[3][3], float from[3]) -{ - to[0] = dot_v3v3(matrix[0], from); - to[1] = dot_v3v3(matrix[1], from); - to[2] = dot_v3v3(matrix[2], from); -} -/* 3x3 matrix addition with 3x3 matrix */ -DO_INLINE void add_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3]) -{ - VECADD(to[0], matrixA[0], matrixB[0]); - VECADD(to[1], matrixA[1], matrixB[1]); - VECADD(to[2], matrixA[2], matrixB[2]); -} -/* A -= B*x + C*y (3x3 matrix sub-addition with 3x3 matrix) */ -DO_INLINE void subadd_fmatrixS_fmatrixS(float to[3][3], float matrixA[3][3], float aS, float matrixB[3][3], float bS) -{ - VECSUBADDSS(to[0], matrixA[0], aS, matrixB[0], bS); - VECSUBADDSS(to[1], matrixA[1], aS, matrixB[1], bS); - VECSUBADDSS(to[2], matrixA[2], aS, matrixB[2], bS); -} -/* A = B - C (3x3 matrix subtraction with 3x3 matrix) */ -DO_INLINE void sub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3]) -{ - sub_v3_v3v3(to[0], matrixA[0], matrixB[0]); - sub_v3_v3v3(to[1], matrixA[1], matrixB[1]); - sub_v3_v3v3(to[2], matrixA[2], matrixB[2]); -} -///////////////////////////////////////////////////////////////// -// special functions -///////////////////////////////////////////////////////////////// -/* 3x3 matrix multiplied+added by a vector */ -/* STATUS: verified */ -DO_INLINE void muladd_fmatrix_fvector(float to[3], float matrix[3][3], float from[3]) -{ - to[0] += dot_v3v3(matrix[0], from); - to[1] += dot_v3v3(matrix[1], from); - to[2] += dot_v3v3(matrix[2], from); -} -///////////////////////////////////////////////////////////////// - -/////////////////////////// -// SPARSE SYMMETRIC big matrix with 3x3 matrix entries -/////////////////////////// -/* printf a big matrix on console: for debug output */ -#if 0 -static void print_bfmatrix(fmatrix3x3 *m3) -{ - unsigned int i = 0; - - for (i = 0; i < m3[0].vcount + m3[0].scount; i++) - { - print_fmatrix(m3[i].m); - } -} -#endif - -/* create big matrix */ -DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs) -{ - // TODO: check if memory allocation was successful */ - fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN(sizeof(fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix"); - temp[0].vcount = verts; - temp[0].scount = springs; - return temp; -} -/* delete big matrix */ -DO_INLINE void del_bfmatrix(fmatrix3x3 *matrix) -{ - if (matrix != NULL) { - MEM_freeN(matrix); - } -} - -/* copy big matrix */ -DO_INLINE void cp_bfmatrix(fmatrix3x3 *to, fmatrix3x3 *from) -{ - // TODO bounds checking - memcpy(to, from, sizeof(fmatrix3x3) * (from[0].vcount+from[0].scount)); -} - -/* init big matrix */ -// slow in parallel -DO_INLINE void init_bfmatrix(fmatrix3x3 *matrix, float m3[3][3]) -{ - unsigned int i; - - for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) { - cp_fmatrix(matrix[i].m, m3); - } -} - -/* init the diagonal of big matrix */ -// slow in parallel -DO_INLINE void initdiag_bfmatrix(fmatrix3x3 *matrix, float m3[3][3]) -{ - unsigned int i, j; - float tmatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - - for (i = 0; i < matrix[0].vcount; i++) { - cp_fmatrix(matrix[i].m, m3); - } - for (j = matrix[0].vcount; j < matrix[0].vcount+matrix[0].scount; j++) { - cp_fmatrix(matrix[j].m, tmatrix); - } -} - -/* SPARSE SYMMETRIC multiply big matrix with long vector*/ -/* STATUS: verified */ -DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector *fLongVector) -{ - unsigned int i = 0; - unsigned int vcount = from[0].vcount; - lfVector *temp = create_lfvector(vcount); - - zero_lfvector(to, vcount); - -#pragma omp parallel sections private(i) if (vcount > CLOTH_OPENMP_LIMIT) - { -#pragma omp section - { - for (i = from[0].vcount; i < from[0].vcount+from[0].scount; i++) { - muladd_fmatrix_fvector(to[from[i].c], from[i].m, fLongVector[from[i].r]); - } - } -#pragma omp section - { - for (i = 0; i < from[0].vcount+from[0].scount; i++) { - muladd_fmatrix_fvector(temp[from[i].r], from[i].m, fLongVector[from[i].c]); - } - } - } - add_lfvector_lfvector(to, to, temp, from[0].vcount); - - del_lfvector(temp); - - -} - -/* SPARSE SYMMETRIC sub big matrix with big matrix*/ -/* A -= B * float + C * float --> for big matrix */ -/* VERIFIED */ -DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, float aS, fmatrix3x3 *matrix, float bS) -{ - unsigned int i = 0; - - /* process diagonal elements */ - for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) { - subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS); - } - -} - -/////////////////////////////////////////////////////////////////// -// simulator start -/////////////////////////////////////////////////////////////////// -typedef struct RootTransform { - float loc[3]; - float rot[3][3]; - - float vel[3]; - float omega[3]; - - float acc[3]; - float domega_dt[3]; -} RootTransform; - -typedef struct Implicit_Data { - /* inputs */ - fmatrix3x3 *bigI; /* identity (constant) */ - fmatrix3x3 *M; /* masses */ - lfVector *F; /* forces */ - fmatrix3x3 *dFdV, *dFdX; /* force jacobians */ - RootTransform *root; /* root transforms */ - - /* motion state data */ - lfVector *X, *Xnew; /* positions */ - lfVector *V, *Vnew; /* velocities */ - - /* internal solver data */ - lfVector *B; /* B for A*dV = B */ - fmatrix3x3 *A; /* A for A*dV = B */ - - lfVector *dV; /* velocity change (solution of A*dV = B) */ - lfVector *z; /* target velocity in constrained directions */ - fmatrix3x3 *S; /* filtering matrix for constraints */ - fmatrix3x3 *P, *Pinv; /* pre-conditioning matrix */ -} Implicit_Data; - -int implicit_init(Object *UNUSED(ob), ClothModifierData *clmd) -{ - unsigned int i = 0; - Cloth *cloth = NULL; - ClothVertex *verts = NULL; - ClothSpring *spring = NULL; - Implicit_Data *id = NULL; - LinkNode *search = NULL; - - if (G.debug_value > 0) - printf("implicit_init\n"); - - // init memory guard - // BLI_listbase_clear(&MEMORY_BASE); - - cloth = (Cloth *)clmd->clothObject; - verts = cloth->verts; - - // create implicit base - id = (Implicit_Data *)MEM_callocN(sizeof(Implicit_Data), "implicit vecmat"); - cloth->implicit = id; - - /* process diagonal elements */ - id->A = create_bfmatrix(cloth->numverts, cloth->numsprings); - id->dFdV = create_bfmatrix(cloth->numverts, cloth->numsprings); - id->dFdX = create_bfmatrix(cloth->numverts, cloth->numsprings); - id->S = create_bfmatrix(cloth->numverts, 0); - id->Pinv = create_bfmatrix(cloth->numverts, cloth->numsprings); - id->P = create_bfmatrix(cloth->numverts, cloth->numsprings); - id->bigI = create_bfmatrix(cloth->numverts, cloth->numsprings); // TODO 0 springs - id->M = create_bfmatrix(cloth->numverts, cloth->numsprings); - id->X = create_lfvector(cloth->numverts); - id->Xnew = create_lfvector(cloth->numverts); - id->V = create_lfvector(cloth->numverts); - id->Vnew = create_lfvector(cloth->numverts); - id->F = create_lfvector(cloth->numverts); - id->B = create_lfvector(cloth->numverts); - id->dV = create_lfvector(cloth->numverts); - id->z = create_lfvector(cloth->numverts); - - id->root = MEM_callocN(sizeof(RootTransform) * cloth->numverts, "root transforms"); - - for (i = 0; i < cloth->numverts; i++) { - 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 = id->M[i].r = id->M[i].c = i; - - initdiag_fmatrixS(id->M[i].m, verts[i].mass); - } - - // init springs - search = cloth->springs; - for (i = 0; i < cloth->numsprings; i++) { - spring = search->link; - - // dFdV_start[i].r = big_I[i].r = big_zero[i].r = - id->A[i+cloth->numverts].r = id->dFdV[i+cloth->numverts].r = id->dFdX[i+cloth->numverts].r = - id->P[i+cloth->numverts].r = id->Pinv[i+cloth->numverts].r = id->bigI[i+cloth->numverts].r = id->M[i+cloth->numverts].r = spring->ij; - - // dFdV_start[i].c = big_I[i].c = big_zero[i].c = - id->A[i+cloth->numverts].c = id->dFdV[i+cloth->numverts].c = id->dFdX[i+cloth->numverts].c = - id->P[i+cloth->numverts].c = id->Pinv[i+cloth->numverts].c = id->bigI[i+cloth->numverts].c = id->M[i+cloth->numverts].c = spring->kl; - - spring->matrix_index = i + cloth->numverts; - - search = search->next; - } - - initdiag_bfmatrix(id->bigI, I); - - for (i = 0; i < cloth->numverts; i++) { - copy_v3_v3(id->X[i], verts[i].x); - } - - return 1; -} - -int implicit_free(ClothModifierData *clmd) -{ - Implicit_Data *id; - Cloth *cloth; - cloth = (Cloth *)clmd->clothObject; - - if (cloth) { - id = cloth->implicit; - - if (id) { - del_bfmatrix(id->A); - del_bfmatrix(id->dFdV); - del_bfmatrix(id->dFdX); - del_bfmatrix(id->S); - del_bfmatrix(id->P); - del_bfmatrix(id->Pinv); - del_bfmatrix(id->bigI); - del_bfmatrix(id->M); - - del_lfvector(id->X); - del_lfvector(id->Xnew); - del_lfvector(id->V); - del_lfvector(id->Vnew); - del_lfvector(id->F); - del_lfvector(id->B); - del_lfvector(id->dV); - del_lfvector(id->z); - - MEM_freeN(id->root); - - MEM_freeN(id); - } - } - - return 1; -} - -/* ==== Transformation of Moving Reference Frame ==== - * x_world, v_world, f_world, a_world, dfdx_world, dfdv_world : state variables in world space - * x_root, v_root, f_root, a_root, dfdx_root, dfdv_root : state variables in root space - * - * x0 : translation of the root frame (hair root location) - * v0 : linear velocity of the root frame - * a0 : acceleration of the root frame - * R : rotation matrix of the root frame - * w : angular velocity of the root frame - * dwdt : angular acceleration of the root frame - */ - -/* x_root = R^T * x_world */ -BLI_INLINE void loc_world_to_root(float r[3], const float v[3], const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - sub_v3_v3v3(r, v, root->loc); - mul_transposed_m3_v3((float (*)[3])root->rot, r); -#else - copy_v3_v3(r, v); - (void)root; -#endif -} - -/* x_world = R * x_root */ -BLI_INLINE void loc_root_to_world(float r[3], const float v[3], const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - copy_v3_v3(r, v); - mul_m3_v3((float (*)[3])root->rot, r); - add_v3_v3(r, root->loc); -#else - copy_v3_v3(r, v); - (void)root; -#endif -} - -/* v_root = cross(w, x_root) + R^T*(v_world - v0) */ -BLI_INLINE void vel_world_to_root(float r[3], const float x_root[3], const float v[3], const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float angvel[3]; - cross_v3_v3v3(angvel, root->omega, x_root); - - sub_v3_v3v3(r, v, root->vel); - mul_transposed_m3_v3((float (*)[3])root->rot, r); - add_v3_v3(r, angvel); -#else - copy_v3_v3(r, v); - (void)x_root; - (void)root; -#endif -} - -/* v_world = R*(v_root - cross(w, x_root)) + v0 */ -BLI_INLINE void vel_root_to_world(float r[3], const float x_root[3], const float v[3], const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float angvel[3]; - cross_v3_v3v3(angvel, root->omega, x_root); - - sub_v3_v3v3(r, v, angvel); - mul_m3_v3((float (*)[3])root->rot, r); - add_v3_v3(r, root->vel); -#else - copy_v3_v3(r, v); - (void)x_root; - (void)root; -#endif -} - -/* a_root = -cross(dwdt, x_root) - 2*cross(w, v_root) - cross(w, cross(w, x_root)) + R^T*(a_world - a0) */ -BLI_INLINE void force_world_to_root(float r[3], const float x_root[3], const float v_root[3], const float force[3], float mass, const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float euler[3], coriolis[3], centrifugal[3], rotvel[3]; - - cross_v3_v3v3(euler, root->domega_dt, x_root); - cross_v3_v3v3(coriolis, root->omega, v_root); - mul_v3_fl(coriolis, 2.0f); - cross_v3_v3v3(rotvel, root->omega, x_root); - cross_v3_v3v3(centrifugal, root->omega, rotvel); - - madd_v3_v3v3fl(r, force, root->acc, mass); - mul_transposed_m3_v3((float (*)[3])root->rot, r); - madd_v3_v3fl(r, euler, mass); - madd_v3_v3fl(r, coriolis, mass); - madd_v3_v3fl(r, centrifugal, mass); -#else - copy_v3_v3(r, force); - (void)x_root; - (void)v_root; - (void)mass; - (void)root; -#endif -} - -/* a_world = R*[ a_root + cross(dwdt, x_root) + 2*cross(w, v_root) + cross(w, cross(w, x_root)) ] + a0 */ -BLI_INLINE void force_root_to_world(float r[3], const float x_root[3], const float v_root[3], const float force[3], float mass, const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float euler[3], coriolis[3], centrifugal[3], rotvel[3]; - - cross_v3_v3v3(euler, root->domega_dt, x_root); - cross_v3_v3v3(coriolis, root->omega, v_root); - mul_v3_fl(coriolis, 2.0f); - cross_v3_v3v3(rotvel, root->omega, x_root); - cross_v3_v3v3(centrifugal, root->omega, rotvel); - - madd_v3_v3v3fl(r, force, euler, mass); - madd_v3_v3fl(r, coriolis, mass); - madd_v3_v3fl(r, centrifugal, mass); - mul_m3_v3((float (*)[3])root->rot, r); - madd_v3_v3fl(r, root->acc, mass); -#else - copy_v3_v3(r, force); - (void)x_root; - (void)v_root; - (void)mass; - (void)root; -#endif -} - -BLI_INLINE void acc_world_to_root(float r[3], const float x_root[3], const float v_root[3], const float acc[3], const RootTransform *root) -{ - force_world_to_root(r, x_root, v_root, acc, 1.0f, root); -} - -BLI_INLINE void acc_root_to_world(float r[3], const float x_root[3], const float v_root[3], const float acc[3], const RootTransform *root) -{ - force_root_to_world(r, x_root, v_root, acc, 1.0f, root); -} - -BLI_INLINE void cross_m3_v3m3(float r[3][3], const float v[3], float m[3][3]) -{ - cross_v3_v3v3(r[0], v, m[0]); - cross_v3_v3v3(r[1], v, m[1]); - cross_v3_v3v3(r[2], v, m[2]); -} - -BLI_INLINE void cross_v3_identity(float r[3][3], const float v[3]) -{ - r[0][0] = 0.0f; r[1][0] = v[2]; r[2][0] = -v[1]; - r[0][1] = -v[2]; r[1][1] = 0.0f; r[2][1] = v[0]; - r[0][2] = v[1]; r[1][2] = -v[0]; r[2][2] = 0.0f; -} - -/* dfdx_root = m*[ -cross(dwdt, I) - cross(w, cross(w, I)) ] + R^T*(dfdx_world) */ -BLI_INLINE void dfdx_world_to_root(float m[3][3], float dfdx[3][3], float mass, const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float t[3][3], u[3][3]; - - copy_m3_m3(t, (float (*)[3])root->rot); - transpose_m3(t); - mul_m3_m3m3(m, t, dfdx); - - cross_v3_identity(t, root->domega_dt); - mul_m3_fl(t, mass); - sub_m3_m3m3(m, m, t); - - cross_v3_identity(u, root->omega); - cross_m3_v3m3(t, root->omega, u); - mul_m3_fl(t, mass); - sub_m3_m3m3(m, m, t); -#else - copy_m3_m3(m, dfdx); - (void)mass; - (void)root; -#endif -} - -/* dfdx_world = R*(dfdx_root + m*[ cross(dwdt, I) + cross(w, cross(w, I)) ]) */ -BLI_INLINE void dfdx_root_to_world(float m[3][3], float dfdx[3][3], float mass, const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float t[3][3], u[3][3]; - - cross_v3_identity(t, root->domega_dt); - mul_m3_fl(t, mass); - add_m3_m3m3(m, dfdx, t); - - cross_v3_identity(u, root->omega); - cross_m3_v3m3(t, root->omega, u); - mul_m3_fl(t, mass); - add_m3_m3m3(m, m, t); - - mul_m3_m3m3(m, (float (*)[3])root->rot, m); -#else - copy_m3_m3(m, dfdx); - (void)mass; - (void)root; -#endif -} - -/* dfdv_root = -2*m*cross(w, I) + R^T*(dfdv_world) */ -BLI_INLINE void dfdv_world_to_root(float m[3][3], float dfdv[3][3], float mass, const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float t[3][3]; - - copy_m3_m3(t, (float (*)[3])root->rot); - transpose_m3(t); - mul_m3_m3m3(m, t, dfdv); - - cross_v3_identity(t, root->omega); - mul_m3_fl(t, 2.0f*mass); - sub_m3_m3m3(m, m, t); -#else - copy_m3_m3(m, dfdv); - (void)mass; - (void)root; -#endif -} - -/* dfdv_world = R*(dfdv_root + 2*m*cross(w, I)) */ -BLI_INLINE void dfdv_root_to_world(float m[3][3], float dfdv[3][3], float mass, const RootTransform *root) -{ -#ifdef CLOTH_ROOT_FRAME - float t[3][3]; - - cross_v3_identity(t, root->omega); - mul_m3_fl(t, 2.0f*mass); - add_m3_m3m3(m, dfdv, t); - - mul_m3_m3m3(m, (float (*)[3])root->rot, m); -#else - copy_m3_m3(m, dfdv); - (void)mass; - (void)root; -#endif -} - -/* ================================ */ - -DO_INLINE float fb(float length, float L) -{ - float x = length / L; - float xx = x * x; - float xxx = xx * x; - float xxxx = xxx * x; - return (-11.541f * xxxx + 34.193f * xxx - 39.083f * xx + 23.116f * x - 9.713f); -} - -DO_INLINE float fbderiv(float length, float L) -{ - float x = length/L; - float xx = x * x; - float xxx = xx * x; - return (-46.164f * xxx + 102.579f * xx - 78.166f * x + 23.116f); -} - -DO_INLINE float fbstar(float length, float L, float kb, float cb) -{ - float tempfb_fl = kb * fb(length, L); - float fbstar_fl = cb * (length - L); - - if (tempfb_fl < fbstar_fl) - return fbstar_fl; - else - return tempfb_fl; -} - -// function to calculae bending spring force (taken from Choi & Co) -DO_INLINE float fbstar_jacobi(float length, float L, float kb, float cb) -{ - float tempfb_fl = kb * fb(length, L); - float fbstar_fl = cb * (length - L); - - if (tempfb_fl < fbstar_fl) { - return cb; - } - else { - return kb * fbderiv(length, L); - } -} - -DO_INLINE void filter(lfVector *V, fmatrix3x3 *S) -{ - unsigned int i=0; - - for (i = 0; i < S[0].vcount; i++) { - mul_m3_v3(S[i].m, V[S[i].r]); - } -} - -#if 0 /* this version of the CG algorithm does not work very well with partial constraints (where S has non-zero elements) */ -static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S) -{ - // Solves for unknown X in equation AX=B - unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100; - float conjgrad_epsilon=0.0001f /* , conjgrad_lasterror=0 */ /* UNUSED */; - lfVector *q, *d, *tmp, *r; - float s, starget, a, s_prev; - unsigned int numverts = lA[0].vcount; - q = create_lfvector(numverts); - d = create_lfvector(numverts); - tmp = create_lfvector(numverts); - r = create_lfvector(numverts); - - // zero_lfvector(ldV, CLOTHPARTICLES); - filter(ldV, S); - - add_lfvector_lfvector(ldV, ldV, z, numverts); - - // r = B - Mul(tmp, A, X); // just use B if X known to be zero - cp_lfvector(r, lB, numverts); - mul_bfmatrix_lfvector(tmp, lA, ldV); - sub_lfvector_lfvector(r, r, tmp, numverts); - - filter(r, S); - - cp_lfvector(d, r, numverts); - - s = dot_lfvector(r, r, numverts); - starget = s * sqrtf(conjgrad_epsilon); - - while (s>starget && conjgrad_loopcount < conjgrad_looplimit) { - // Mul(q, A, d); // q = A*d; - mul_bfmatrix_lfvector(q, lA, d); - - filter(q, S); - - a = s/dot_lfvector(d, q, numverts); - - // X = X + d*a; - add_lfvector_lfvectorS(ldV, ldV, d, a, numverts); - - // r = r - q*a; - sub_lfvector_lfvectorS(r, r, q, a, numverts); - - s_prev = s; - s = dot_lfvector(r, r, numverts); - - //d = r+d*(s/s_prev); - add_lfvector_lfvectorS(d, r, d, (s/s_prev), numverts); - - filter(d, S); - - conjgrad_loopcount++; - } - /* conjgrad_lasterror = s; */ /* UNUSED */ - - del_lfvector(q); - del_lfvector(d); - del_lfvector(tmp); - del_lfvector(r); - // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount); - - return conjgrad_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable -} -#endif - -static int cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S) -{ - // Solves for unknown X in equation AX=B - unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100; - float conjgrad_epsilon=0.01f; - - unsigned int numverts = lA[0].vcount; - lfVector *fB = create_lfvector(numverts); - lfVector *AdV = create_lfvector(numverts); - lfVector *r = create_lfvector(numverts); - lfVector *c = create_lfvector(numverts); - lfVector *q = create_lfvector(numverts); - lfVector *s = create_lfvector(numverts); - float delta_new, delta_old, delta_target, alpha; - - cp_lfvector(ldV, z, numverts); - - /* d0 = filter(B)^T * P * filter(B) */ - cp_lfvector(fB, lB, numverts); - filter(fB, S); - delta_target = conjgrad_epsilon*conjgrad_epsilon * dot_lfvector(fB, fB, numverts); - - /* r = filter(B - A * dV) */ - mul_bfmatrix_lfvector(AdV, lA, ldV); - sub_lfvector_lfvector(r, lB, AdV, numverts); - filter(r, S); - - /* c = filter(P^-1 * r) */ - cp_lfvector(c, r, numverts); - filter(c, S); - - /* delta = r^T * c */ - delta_new = dot_lfvector(r, c, numverts); - -#ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT - printf("==== A ====\n"); - print_bfmatrix(lA); - printf("==== z ====\n"); - print_lvector(z, numverts); - printf("==== B ====\n"); - print_lvector(lB, numverts); - printf("==== S ====\n"); - print_bfmatrix(S); -#endif - - while (delta_new > delta_target && conjgrad_loopcount < conjgrad_looplimit) { - mul_bfmatrix_lfvector(q, lA, c); - filter(q, S); - - alpha = delta_new / dot_lfvector(c, q, numverts); - - add_lfvector_lfvectorS(ldV, ldV, c, alpha, numverts); - - add_lfvector_lfvectorS(r, r, q, -alpha, numverts); - - /* s = P^-1 * r */ - cp_lfvector(s, r, numverts); - delta_old = delta_new; - delta_new = dot_lfvector(r, s, numverts); - - add_lfvector_lfvectorS(c, s, c, delta_new / delta_old, numverts); - filter(c, S); - - conjgrad_loopcount++; - } - -#ifdef IMPLICIT_PRINT_SOLVER_INPUT_OUTPUT - printf("==== dV ====\n"); - print_lvector(ldV, numverts); - printf("========\n"); -#endif - - del_lfvector(fB); - del_lfvector(AdV); - del_lfvector(r); - del_lfvector(c); - del_lfvector(q); - del_lfvector(s); - // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount); - - return conjgrad_loopcount < conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable -} - -#if 0 -// block diagonalizer -DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv) -{ - unsigned int i = 0; - - // Take only the diagonal blocks of A -// #pragma omp parallel for private(i) if (lA[0].vcount > CLOTH_OPENMP_LIMIT) - for (i = 0; i<lA[0].vcount; i++) { - // block diagonalizer - cp_fmatrix(P[i].m, lA[i].m); - inverse_fmatrix(Pinv[i].m, P[i].m); - - } -} -/* -// version 1.3 -static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv) -{ - unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100; - float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0; - float conjgrad_epsilon=0.0001; // 0.2 is dt for steps=5 - lfVector *r = create_lfvector(numverts); - lfVector *p = create_lfvector(numverts); - lfVector *s = create_lfvector(numverts); - lfVector *h = create_lfvector(numverts); - - BuildPPinv(lA, P, Pinv); - - filter(dv, S); - add_lfvector_lfvector(dv, dv, z, numverts); - - mul_bfmatrix_lfvector(r, lA, dv); - sub_lfvector_lfvector(r, lB, r, numverts); - filter(r, S); - - mul_prevfmatrix_lfvector(p, Pinv, r); - filter(p, S); - - deltaNew = dot_lfvector(r, p, numverts); - - delta0 = deltaNew * sqrt(conjgrad_epsilon); - - // itstart(); - - while ((deltaNew > delta0) && (iterations < conjgrad_looplimit)) - { - iterations++; - - mul_bfmatrix_lfvector(s, lA, p); - filter(s, S); - - alpha = deltaNew / dot_lfvector(p, s, numverts); - - add_lfvector_lfvectorS(dv, dv, p, alpha, numverts); - - add_lfvector_lfvectorS(r, r, s, -alpha, numverts); - - mul_prevfmatrix_lfvector(h, Pinv, r); - filter(h, S); - - deltaOld = deltaNew; - - deltaNew = dot_lfvector(r, h, numverts); - - add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts); - - filter(p, S); - - } - - // itend(); - // printf("cg_filtered_pre time: %f\n", (float)itval()); - - del_lfvector(h); - del_lfvector(s); - del_lfvector(p); - del_lfvector(r); - - printf("iterations: %d\n", iterations); - - return iterations<conjgrad_looplimit; -} -*/ -// version 1.4 -static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *bigI) -{ - unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100; - float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0, tol = 0; - lfVector *r = create_lfvector(numverts); - lfVector *p = create_lfvector(numverts); - lfVector *s = create_lfvector(numverts); - lfVector *h = create_lfvector(numverts); - lfVector *bhat = create_lfvector(numverts); - lfVector *btemp = create_lfvector(numverts); - - BuildPPinv(lA, P, Pinv); - - initdiag_bfmatrix(bigI, I); - sub_bfmatrix_Smatrix(bigI, bigI, S); - - // x = Sx_0+(I-S)z - filter(dv, S); - add_lfvector_lfvector(dv, dv, z, numverts); - - // b_hat = S(b-A(I-S)z) - mul_bfmatrix_lfvector(r, lA, z); - mul_bfmatrix_lfvector(bhat, bigI, r); - sub_lfvector_lfvector(bhat, lB, bhat, numverts); - - // r = S(b-Ax) - mul_bfmatrix_lfvector(r, lA, dv); - sub_lfvector_lfvector(r, lB, r, numverts); - filter(r, S); - - // p = SP^-1r - mul_prevfmatrix_lfvector(p, Pinv, r); - filter(p, S); - - // delta0 = bhat^TP^-1bhat - mul_prevfmatrix_lfvector(btemp, Pinv, bhat); - delta0 = dot_lfvector(bhat, btemp, numverts); - - // deltaNew = r^TP - deltaNew = dot_lfvector(r, p, numverts); - - /* - filter(dv, S); - add_lfvector_lfvector(dv, dv, z, numverts); - - mul_bfmatrix_lfvector(r, lA, dv); - sub_lfvector_lfvector(r, lB, r, numverts); - filter(r, S); - - mul_prevfmatrix_lfvector(p, Pinv, r); - filter(p, S); - - deltaNew = dot_lfvector(r, p, numverts); - - delta0 = deltaNew * sqrt(conjgrad_epsilon); - */ - - // itstart(); - - tol = (0.01*0.2); - - while ((deltaNew > delta0*tol*tol) && (iterations < conjgrad_looplimit)) - { - iterations++; - - mul_bfmatrix_lfvector(s, lA, p); - filter(s, S); - - alpha = deltaNew / dot_lfvector(p, s, numverts); - - add_lfvector_lfvectorS(dv, dv, p, alpha, numverts); - - add_lfvector_lfvectorS(r, r, s, -alpha, numverts); - - mul_prevfmatrix_lfvector(h, Pinv, r); - filter(h, S); - - deltaOld = deltaNew; - - deltaNew = dot_lfvector(r, h, numverts); - - add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts); - - filter(p, S); - - } - - // itend(); - // printf("cg_filtered_pre time: %f\n", (float)itval()); - - del_lfvector(btemp); - del_lfvector(bhat); - del_lfvector(h); - del_lfvector(s); - del_lfvector(p); - del_lfvector(r); - - // printf("iterations: %d\n", iterations); - - return iterations<conjgrad_looplimit; -} -#endif - -DO_INLINE void dfdx_spring_type2(float to[3][3], float dir[3], float length, float L, float k, float cb) -{ - // return outerprod(dir, dir)*fbstar_jacobi(length, L, k, cb); - mul_fvectorT_fvectorS(to, dir, dir, fbstar_jacobi(length, L, k, cb)); -} - -DO_INLINE void dfdv_damp(float to[3][3], float dir[3], float damping) -{ - // derivative of force wrt velocity. - mul_fvectorT_fvectorS(to, dir, dir, damping); - -} - -DO_INLINE void dfdx_spring(float to[3][3], float dir[3], float length, float L, float k) -{ - // dir is unit length direction, rest is spring's restlength, k is spring constant. - //return ( (I-outerprod(dir, dir))*Min(1.0f, rest/length) - I) * -k; - mul_fvectorT_fvector(to, dir, dir); - sub_fmatrix_fmatrix(to, I, to); - - mul_fmatrix_S(to, (L/length)); - sub_fmatrix_fmatrix(to, to, I); - mul_fmatrix_S(to, -k); -} - -DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *UNUSED(lF), lfVector *X, lfVector *V, fmatrix3x3 *UNUSED(dFdV), fmatrix3x3 *UNUSED(dFdX), float time) -{ - Cloth *cloth = clmd->clothObject; - ClothVertex *verts = cloth->verts; - float extent[3]; - float length = 0, dot = 0; - float dir[3] = {0, 0, 0}; - float vel[3]; - float k = 0.0f; - float L = s->restlen; - float cb; /* = clmd->sim_parms->structural; */ /*UNUSED*/ - - float nullf[3] = {0, 0, 0}; - float stretch_force[3] = {0, 0, 0}; - float bending_force[3] = {0, 0, 0}; - float damping_force[3] = {0, 0, 0}; - float nulldfdx[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - - float scaling = 0.0; - - int no_compress = clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_NO_SPRING_COMPRESS; - - copy_v3_v3(s->f, nullf); - cp_fmatrix(s->dfdx, nulldfdx); - cp_fmatrix(s->dfdv, nulldfdx); - - // calculate elonglation - sub_v3_v3v3(extent, X[s->kl], X[s->ij]); - sub_v3_v3v3(vel, V[s->kl], V[s->ij]); - dot = dot_v3v3(extent, extent); - length = sqrtf(dot); - - s->flags &= ~CLOTH_SPRING_FLAG_NEEDED; - - if (length > ALMOST_ZERO) { - /* - if (length>L) - { - if ((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && - ((((length-L)*100.0f/L) > clmd->sim_parms->maxspringlen))) // cut spring! - { - s->flags |= CSPRING_FLAG_DEACTIVATE; - return; - } - } - */ - mul_fvector_S(dir, extent, 1.0f/length); - } - else { - mul_fvector_S(dir, extent, 0.0f); - } - - // calculate force of structural + shear springs - if ((s->type & CLOTH_SPRING_TYPE_STRUCTURAL) || (s->type & CLOTH_SPRING_TYPE_SHEAR) || (s->type & CLOTH_SPRING_TYPE_SEWING) ) { -#ifdef CLOTH_FORCE_SPRING_STRUCTURAL - if (length > L || no_compress) { - s->flags |= CLOTH_SPRING_FLAG_NEEDED; - - k = clmd->sim_parms->structural; - - scaling = k + s->stiffness * fabsf(clmd->sim_parms->max_struct - k); - - k = scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON); - - // TODO: verify, half verified (couldn't see error) - if (s->type & CLOTH_SPRING_TYPE_SEWING) { - // sewing springs usually have a large distance at first so clamp the force so we don't get tunnelling through colission objects - float force = k*(length-L); - if (force > clmd->sim_parms->max_sewing) { - force = clmd->sim_parms->max_sewing; - } - mul_fvector_S(stretch_force, dir, force); - } - else { - mul_fvector_S(stretch_force, dir, k * (length - L)); - } - - VECADD(s->f, s->f, stretch_force); - - // Ascher & Boxman, p.21: Damping only during elonglation - // something wrong with it... - mul_fvector_S(damping_force, dir, clmd->sim_parms->Cdis * dot_v3v3(vel, dir)); - VECADD(s->f, s->f, damping_force); - - /* VERIFIED */ - dfdx_spring(s->dfdx, dir, length, L, k); - - /* VERIFIED */ - dfdv_damp(s->dfdv, dir, clmd->sim_parms->Cdis); - - } -#endif - } - else if (s->type & CLOTH_SPRING_TYPE_GOAL) { -#ifdef CLOTH_FORCE_SPRING_GOAL - float tvect[3]; - - s->flags |= CLOTH_SPRING_FLAG_NEEDED; - - // current_position = xold + t * (newposition - xold) - sub_v3_v3v3(tvect, verts[s->ij].xconst, verts[s->ij].xold); - mul_fvector_S(tvect, tvect, time); - VECADD(tvect, tvect, verts[s->ij].xold); - - sub_v3_v3v3(extent, X[s->ij], tvect); - - // SEE MSG BELOW (these are UNUSED) - // dot = dot_v3v3(extent, extent); - // length = sqrt(dot); - - k = clmd->sim_parms->goalspring; - - scaling = k + s->stiffness * fabsf(clmd->sim_parms->max_struct - k); - - k = verts [s->ij].goal * scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON); - - VECADDS(s->f, s->f, extent, -k); - - mul_fvector_S(damping_force, dir, clmd->sim_parms->goalfrict * 0.01f * dot_v3v3(vel, dir)); - VECADD(s->f, s->f, damping_force); - - // HERE IS THE PROBLEM!!!! - // dfdx_spring(s->dfdx, dir, length, 0.0, k); - // dfdv_damp(s->dfdv, dir, MIN2(1.0, (clmd->sim_parms->goalfrict/100.0))); -#endif - } - else { /* calculate force of bending springs */ -#ifdef CLOTH_FORCE_SPRING_BEND - if (length < L) { - s->flags |= CLOTH_SPRING_FLAG_NEEDED; - - k = clmd->sim_parms->bending; - - scaling = k + s->stiffness * fabsf(clmd->sim_parms->max_bend - k); - cb = k = scaling / (20.0f * (clmd->sim_parms->avg_spring_len + FLT_EPSILON)); - - mul_fvector_S(bending_force, dir, fbstar(length, L, k, cb)); - VECADD(s->f, s->f, bending_force); - - dfdx_spring_type2(s->dfdx, dir, length, L, k, cb); - } -#endif - } -} - -DO_INLINE void cloth_apply_spring_force(ClothModifierData *UNUSED(clmd), ClothSpring *s, lfVector *lF, lfVector *UNUSED(X), lfVector *UNUSED(V), fmatrix3x3 *dFdV, fmatrix3x3 *dFdX) -{ - if (s->flags & CLOTH_SPRING_FLAG_NEEDED) { - if (!(s->type & CLOTH_SPRING_TYPE_BENDING)) { - sub_fmatrix_fmatrix(dFdV[s->ij].m, dFdV[s->ij].m, s->dfdv); - sub_fmatrix_fmatrix(dFdV[s->kl].m, dFdV[s->kl].m, s->dfdv); - add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv); - } - - VECADD(lF[s->ij], lF[s->ij], s->f); - - if (!(s->type & CLOTH_SPRING_TYPE_GOAL)) - sub_v3_v3v3(lF[s->kl], lF[s->kl], s->f); - - sub_fmatrix_fmatrix(dFdX[s->kl].m, dFdX[s->kl].m, s->dfdx); - sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx); - add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx); - } -} - - -static void CalcFloat( float *v1, float *v2, float *v3, float *n) -{ - float n1[3], n2[3]; - - n1[0] = v1[0]-v2[0]; - n2[0] = v2[0]-v3[0]; - n1[1] = v1[1]-v2[1]; - n2[1] = v2[1]-v3[1]; - n1[2] = v1[2]-v2[2]; - n2[2] = v2[2]-v3[2]; - n[0] = n1[1]*n2[2]-n1[2]*n2[1]; - n[1] = n1[2]*n2[0]-n1[0]*n2[2]; - n[2] = n1[0]*n2[1]-n1[1]*n2[0]; -} - -static void CalcFloat4( float *v1, float *v2, float *v3, float *v4, float *n) -{ - /* real cross! */ - float n1[3], n2[3]; - - n1[0] = v1[0]-v3[0]; - n1[1] = v1[1]-v3[1]; - n1[2] = v1[2]-v3[2]; - - n2[0] = v2[0]-v4[0]; - n2[1] = v2[1]-v4[1]; - n2[2] = v2[2]-v4[2]; - - n[0] = n1[1]*n2[2]-n1[2]*n2[1]; - n[1] = n1[2]*n2[0]-n1[0]*n2[2]; - n[2] = n1[0]*n2[1]-n1[1]*n2[0]; -} - -static float calculateVertexWindForce(const float wind[3], const float vertexnormal[3]) -{ - return dot_v3v3(wind, vertexnormal); -} - -/* ================ Volumetric Hair Interaction ================ - * adapted from - * Volumetric Methods for Simulation and Rendering of Hair - * by Lena Petrovic, Mark Henne and John Anderson - * Pixar Technical Memo #06-08, Pixar Animation Studios - */ - -/* Note about array indexing: - * Generally the arrays here are one-dimensional. - * The relation between 3D indices and the array offset is - * offset = x + res_x * y + res_y * z - */ - -/* TODO: This is an initial implementation and should be made much better in due time. - * What should at least be implemented is a grid size parameter and a smoothing kernel - * for bigger grids. - */ - -/* 10x10x10 grid gives nice initial results */ -static const int hair_grid_res = 10; - -static int hair_grid_size(int res) -{ - return res * res * res; -} - -BLI_INLINE void hair_grid_get_scale(int res, const float gmin[3], const float gmax[3], float scale[3]) -{ - sub_v3_v3v3(scale, gmax, gmin); - mul_v3_fl(scale, 1.0f / (res-1)); -} - -typedef struct HairGridVert { - float velocity[3]; - float density; -} HairGridVert; - -#define HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, axis) ( min_ii( max_ii( (int)((vec[axis] - gmin[axis]) / scale[axis]), 0), res-2 ) ) - -BLI_INLINE int hair_grid_offset(const float vec[3], int res, const float gmin[3], const float scale[3]) -{ - int i, j, k; - i = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 0); - j = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 1); - k = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 2); - return i + (j + k*res)*res; -} - -BLI_INLINE int hair_grid_interp_weights(int res, const float gmin[3], const float scale[3], const float vec[3], float uvw[3]) -{ - int i, j, k, offset; - - i = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 0); - j = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 1); - k = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 2); - offset = i + (j + k*res)*res; - - uvw[0] = (vec[0] - gmin[0]) / scale[0] - (float)i; - uvw[1] = (vec[1] - gmin[1]) / scale[1] - (float)j; - uvw[2] = (vec[2] - gmin[2]) / scale[2] - (float)k; - - return offset; -} - -BLI_INLINE void hair_grid_interpolate(const HairGridVert *grid, int res, const float gmin[3], const float scale[3], const float vec[3], - float *density, float velocity[3], float density_gradient[3]) -{ - HairGridVert data[8]; - float uvw[3], muvw[3]; - int res2 = res * res; - int offset; - - offset = hair_grid_interp_weights(res, gmin, scale, vec, uvw); - muvw[0] = 1.0f - uvw[0]; - muvw[1] = 1.0f - uvw[1]; - muvw[2] = 1.0f - uvw[2]; - - data[0] = grid[offset ]; - data[1] = grid[offset +1]; - data[2] = grid[offset +res ]; - data[3] = grid[offset +res+1]; - data[4] = grid[offset+res2 ]; - data[5] = grid[offset+res2 +1]; - data[6] = grid[offset+res2+res ]; - data[7] = grid[offset+res2+res+1]; - - if (density) { - *density = muvw[2]*( muvw[1]*( muvw[0]*data[0].density + uvw[0]*data[1].density ) + - uvw[1]*( muvw[0]*data[2].density + uvw[0]*data[3].density ) ) + - uvw[2]*( muvw[1]*( muvw[0]*data[4].density + uvw[0]*data[5].density ) + - uvw[1]*( muvw[0]*data[6].density + uvw[0]*data[7].density ) ); - } - if (velocity) { - int k; - for (k = 0; k < 3; ++k) { - velocity[k] = muvw[2]*( muvw[1]*( muvw[0]*data[0].velocity[k] + uvw[0]*data[1].velocity[k] ) + - uvw[1]*( muvw[0]*data[2].velocity[k] + uvw[0]*data[3].velocity[k] ) ) + - uvw[2]*( muvw[1]*( muvw[0]*data[4].velocity[k] + uvw[0]*data[5].velocity[k] ) + - uvw[1]*( muvw[0]*data[6].velocity[k] + uvw[0]*data[7].velocity[k] ) ); - } - } - if (density_gradient) { - density_gradient[0] = muvw[1] * muvw[2] * ( data[0].density - data[1].density ) + - uvw[1] * muvw[2] * ( data[2].density - data[3].density ) + - muvw[1] * uvw[2] * ( data[4].density - data[5].density ) + - uvw[1] * uvw[2] * ( data[6].density - data[7].density ); - - density_gradient[1] = muvw[2] * muvw[0] * ( data[0].density - data[2].density ) + - uvw[2] * muvw[0] * ( data[4].density - data[6].density ) + - muvw[2] * uvw[0] * ( data[1].density - data[3].density ) + - uvw[2] * uvw[0] * ( data[5].density - data[7].density ); - - density_gradient[2] = muvw[2] * muvw[0] * ( data[0].density - data[4].density ) + - uvw[2] * muvw[0] * ( data[1].density - data[5].density ) + - muvw[2] * uvw[0] * ( data[2].density - data[6].density ) + - uvw[2] * uvw[0] * ( data[3].density - data[7].density ); - } -} - -static void hair_velocity_smoothing(const HairGridVert *hairgrid, const float gmin[3], const float scale[3], float smoothfac, - lfVector *lF, lfVector *lX, lfVector *lV, unsigned int numverts) -{ - int v; - /* calculate forces */ - for (v = 0; v < numverts; v++) { - float density, velocity[3]; - - hair_grid_interpolate(hairgrid, hair_grid_res, gmin, scale, lX[v], &density, velocity, NULL); - - sub_v3_v3(velocity, lV[v]); - madd_v3_v3fl(lF[v], velocity, smoothfac); - } -} - -static void hair_velocity_collision(const HairGridVert *collgrid, const float gmin[3], const float scale[3], float collfac, - lfVector *lF, lfVector *lX, lfVector *lV, unsigned int numverts) -{ - int v; - /* calculate forces */ - for (v = 0; v < numverts; v++) { - int offset = hair_grid_offset(lX[v], hair_grid_res, gmin, scale); - - if (collgrid[offset].density > 0.0f) { - lF[v][0] += collfac * (collgrid[offset].velocity[0] - lV[v][0]); - lF[v][1] += collfac * (collgrid[offset].velocity[1] - lV[v][1]); - lF[v][2] += collfac * (collgrid[offset].velocity[2] - lV[v][2]); - } - } -} - -static void hair_pressure_force(const HairGridVert *hairgrid, const float gmin[3], const float scale[3], float pressurefac, float minpressure, - lfVector *lF, lfVector *lX, unsigned int numverts) -{ - int v; - - /* calculate forces */ - for (v = 0; v < numverts; v++) { - float density, gradient[3], gradlen; - - hair_grid_interpolate(hairgrid, hair_grid_res, gmin, scale, lX[v], &density, NULL, gradient); - - gradlen = normalize_v3(gradient) - minpressure; - if (gradlen < 0.0f) - continue; - mul_v3_fl(gradient, gradlen); - - madd_v3_v3fl(lF[v], gradient, pressurefac); - } -} - -static void hair_volume_get_boundbox(lfVector *lX, unsigned int numverts, float gmin[3], float gmax[3]) -{ - int i; - - INIT_MINMAX(gmin, gmax); - for (i = 0; i < numverts; i++) - DO_MINMAX(lX[i], gmin, gmax); -} - -BLI_INLINE bool hair_grid_point_valid(const float vec[3], float gmin[3], float gmax[3]) -{ - return !(vec[0] < gmin[0] || vec[1] < gmin[1] || vec[2] < gmin[2] || - vec[0] > gmax[0] || vec[1] > gmax[1] || vec[2] > gmax[2]); -} - -BLI_INLINE float dist_tent_v3f3(const float a[3], float x, float y, float z) -{ - float w = (1.0f - fabsf(a[0] - x)) * (1.0f - fabsf(a[1] - y)) * (1.0f - fabsf(a[2] - z)); - return w; -} - -/* returns the grid array offset as well to avoid redundant calculation */ -static int hair_grid_weights(int res, const float gmin[3], const float scale[3], const float vec[3], float weights[8]) -{ - int i, j, k, offset; - float uvw[3]; - - i = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 0); - j = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 1); - k = HAIR_GRID_INDEX_AXIS(vec, res, gmin, scale, 2); - offset = i + (j + k*res)*res; - - uvw[0] = (vec[0] - gmin[0]) / scale[0]; - uvw[1] = (vec[1] - gmin[1]) / scale[1]; - uvw[2] = (vec[2] - gmin[2]) / scale[2]; - - weights[0] = dist_tent_v3f3(uvw, (float)i , (float)j , (float)k ); - weights[1] = dist_tent_v3f3(uvw, (float)(i+1), (float)j , (float)k ); - weights[2] = dist_tent_v3f3(uvw, (float)i , (float)(j+1), (float)k ); - weights[3] = dist_tent_v3f3(uvw, (float)(i+1), (float)(j+1), (float)k ); - weights[4] = dist_tent_v3f3(uvw, (float)i , (float)j , (float)(k+1)); - weights[5] = dist_tent_v3f3(uvw, (float)(i+1), (float)j , (float)(k+1)); - weights[6] = dist_tent_v3f3(uvw, (float)i , (float)(j+1), (float)(k+1)); - weights[7] = dist_tent_v3f3(uvw, (float)(i+1), (float)(j+1), (float)(k+1)); - - return offset; -} - -static HairGridVert *hair_volume_create_hair_grid(ClothModifierData *clmd, lfVector *lX, lfVector *lV, unsigned int numverts) -{ - int res = hair_grid_res; - int size = hair_grid_size(res); - HairGridVert *hairgrid; - float gmin[3], gmax[3], scale[3]; - /* 2.0f is an experimental value that seems to give good results */ - float smoothfac = 2.0f * clmd->sim_parms->velocity_smooth; - unsigned int v = 0; - int i = 0; - - hair_volume_get_boundbox(lX, numverts, gmin, gmax); - hair_grid_get_scale(res, gmin, gmax, scale); - - hairgrid = MEM_mallocN(sizeof(HairGridVert) * size, "hair voxel data"); - - /* initialize grid */ - for (i = 0; i < size; ++i) { - zero_v3(hairgrid[i].velocity); - hairgrid[i].density = 0.0f; - } - - /* gather velocities & density */ - if (smoothfac > 0.0f) { - for (v = 0; v < numverts; v++) { - float *V = lV[v]; - float weights[8]; - int di, dj, dk; - int offset; - - if (!hair_grid_point_valid(lX[v], gmin, gmax)) - continue; - - offset = hair_grid_weights(res, gmin, scale, lX[v], weights); - - for (di = 0; di < 2; ++di) { - for (dj = 0; dj < 2; ++dj) { - for (dk = 0; dk < 2; ++dk) { - int voffset = offset + di + (dj + dk*res)*res; - int iw = di + dj*2 + dk*4; - - hairgrid[voffset].density += weights[iw]; - madd_v3_v3fl(hairgrid[voffset].velocity, V, weights[iw]); - } - } - } - } - } - - /* divide velocity with density */ - for (i = 0; i < size; i++) { - float density = hairgrid[i].density; - if (density > 0.0f) - mul_v3_fl(hairgrid[i].velocity, 1.0f/density); - } - - return hairgrid; -} - - -static HairGridVert *hair_volume_create_collision_grid(ClothModifierData *clmd, lfVector *lX, unsigned int numverts) -{ - int res = hair_grid_res; - int size = hair_grid_size(res); - HairGridVert *collgrid; - ListBase *colliders; - ColliderCache *col = NULL; - float gmin[3], gmax[3], scale[3]; - /* 2.0f is an experimental value that seems to give good results */ - float collfac = 2.0f * clmd->sim_parms->collider_friction; - unsigned int v = 0; - int i = 0; - - hair_volume_get_boundbox(lX, numverts, gmin, gmax); - hair_grid_get_scale(res, gmin, gmax, scale); - - collgrid = MEM_mallocN(sizeof(HairGridVert) * size, "hair collider voxel data"); - - /* initialize grid */ - for (i = 0; i < size; ++i) { - zero_v3(collgrid[i].velocity); - collgrid[i].density = 0.0f; - } - - /* gather colliders */ - colliders = get_collider_cache(clmd->scene, NULL, NULL); - if (colliders && collfac > 0.0f) { - for (col = colliders->first; col; col = col->next) { - MVert *loc0 = col->collmd->x; - MVert *loc1 = col->collmd->xnew; - float vel[3]; - float weights[8]; - int di, dj, dk; - - for (v=0; v < col->collmd->numverts; v++, loc0++, loc1++) { - int offset; - - if (!hair_grid_point_valid(loc1->co, gmin, gmax)) - continue; - - offset = hair_grid_weights(res, gmin, scale, lX[v], weights); - - sub_v3_v3v3(vel, loc1->co, loc0->co); - - for (di = 0; di < 2; ++di) { - for (dj = 0; dj < 2; ++dj) { - for (dk = 0; dk < 2; ++dk) { - int voffset = offset + di + (dj + dk*res)*res; - int iw = di + dj*2 + dk*4; - - collgrid[voffset].density += weights[iw]; - madd_v3_v3fl(collgrid[voffset].velocity, vel, weights[iw]); - } - } - } - } - } - } - free_collider_cache(&colliders); - - /* divide velocity with density */ - for (i = 0; i < size; i++) { - float density = collgrid[i].density; - if (density > 0.0f) - mul_v3_fl(collgrid[i].velocity, 1.0f/density); - } - - return collgrid; -} - -static void hair_volume_forces(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVector *lV, unsigned int numverts) -{ - HairGridVert *hairgrid, *collgrid; - float gmin[3], gmax[3], scale[3]; - /* 2.0f is an experimental value that seems to give good results */ - float smoothfac = 2.0f * clmd->sim_parms->velocity_smooth; - float collfac = 2.0f * clmd->sim_parms->collider_friction; - float pressfac = clmd->sim_parms->pressure; - float minpress = clmd->sim_parms->pressure_threshold; - - if (smoothfac <= 0.0f && collfac <= 0.0f && pressfac <= 0.0f) - return; - - hair_volume_get_boundbox(lX, numverts, gmin, gmax); - hair_grid_get_scale(hair_grid_res, gmin, gmax, scale); - - hairgrid = hair_volume_create_hair_grid(clmd, lX, lV, numverts); - collgrid = hair_volume_create_collision_grid(clmd, lX, numverts); - - hair_velocity_smoothing(hairgrid, gmin, scale, smoothfac, lF, lX, lV, numverts); - hair_velocity_collision(collgrid, gmin, scale, collfac, lF, lX, lV, numverts); - hair_pressure_force(hairgrid, gmin, scale, pressfac, minpress, lF, lX, numverts); - - MEM_freeN(hairgrid); - MEM_freeN(collgrid); -} - -bool implicit_hair_volume_get_texture_data(Object *UNUSED(ob), ClothModifierData *clmd, ListBase *UNUSED(effectors), VoxelData *vd) -{ - lfVector *lX, *lV; - HairGridVert *hairgrid/*, *collgrid*/; - int numverts; - int totres, i; - int depth; - - if (!clmd->clothObject || !clmd->clothObject->implicit) - return false; - - lX = clmd->clothObject->implicit->X; - lV = clmd->clothObject->implicit->V; - numverts = clmd->clothObject->numverts; - - hairgrid = hair_volume_create_hair_grid(clmd, lX, lV, numverts); -// collgrid = hair_volume_create_collision_grid(clmd, lX, numverts); - - vd->resol[0] = hair_grid_res; - vd->resol[1] = hair_grid_res; - vd->resol[2] = hair_grid_res; - - totres = hair_grid_size(hair_grid_res); - - if (vd->hair_type == TEX_VD_HAIRVELOCITY) { - depth = 4; - vd->data_type = TEX_VD_RGBA_PREMUL; - } - else { - depth = 1; - vd->data_type = TEX_VD_INTENSITY; - } - - if (totres > 0) { - vd->dataset = (float *)MEM_mapallocN(sizeof(float) * depth * (totres), "hair volume texture data"); - - for (i = 0; i < totres; ++i) { - switch (vd->hair_type) { - case TEX_VD_HAIRDENSITY: - vd->dataset[i] = hairgrid[i].density; - break; - - case TEX_VD_HAIRRESTDENSITY: - vd->dataset[i] = 0.0f; // TODO - break; - - case TEX_VD_HAIRVELOCITY: - vd->dataset[i + 0*totres] = hairgrid[i].velocity[0]; - vd->dataset[i + 1*totres] = hairgrid[i].velocity[1]; - vd->dataset[i + 2*totres] = hairgrid[i].velocity[2]; - vd->dataset[i + 3*totres] = len_v3(hairgrid[i].velocity); - break; - - case TEX_VD_HAIRENERGY: - vd->dataset[i] = 0.0f; // TODO - break; - } - } - } - else { - vd->dataset = NULL; - } - - MEM_freeN(hairgrid); -// MEM_freeN(collgrid); - - return true; -} - -/* ================================ */ - -static bool collision_response(ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, float restitution, float r_impulse[3]) -{ - Cloth *cloth = clmd->clothObject; - int index = collpair->ap1; - bool result = false; - - float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3]; - float epsilon2 = BLI_bvhtree_getepsilon(collmd->bvhtree); - - float margin_distance = collpair->distance - epsilon2; - float mag_v_rel; - - zero_v3(r_impulse); - - if (margin_distance > 0.0f) - return false; /* XXX tested before already? */ - - /* only handle static collisions here */ - if ( collpair->flag & COLLISION_IN_FUTURE ) - return false; - - /* velocity */ - copy_v3_v3(v1, cloth->verts[index].v); - collision_get_collider_velocity(v2_old, v2_new, collmd, collpair); - /* relative velocity = velocity of the cloth point relative to the collider */ - sub_v3_v3v3(v_rel_old, v1, v2_old); - sub_v3_v3v3(v_rel_new, v1, v2_new); - /* normal component of the relative velocity */ - mag_v_rel = dot_v3v3(v_rel_old, collpair->normal); - - /* only valid when moving toward the collider */ - if (mag_v_rel < -ALMOST_ZERO) { - float v_nor_old, v_nor_new; - float v_tan_old[3], v_tan_new[3]; - float bounce, repulse; - - /* Collision response based on - * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005) - * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf - */ - - v_nor_old = mag_v_rel; - v_nor_new = dot_v3v3(v_rel_new, collpair->normal); - - madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old); - madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new); - - /* TODO repulsion forces can easily destabilize the system, - * have to clamp them or construct a linear spring instead - */ -// repulse = -margin_distance / dt + dot_v3v3(v1, collpair->normal); - repulse = 0.0f; - - if (margin_distance < -epsilon2) { - bounce = -(v_nor_new + v_nor_old * restitution); - mul_v3_v3fl(r_impulse, collpair->normal, max_ff(repulse, bounce)); - } - else { - bounce = 0.0f; - mul_v3_v3fl(r_impulse, collpair->normal, repulse); - } - - result = true; - } - - return result; -} - -/* Init constraint matrix - * This is part of the modified CG method suggested by Baraff/Witkin in - * "Large Steps in Cloth Simulation" (Siggraph 1998) - */ -static void setup_constraint_matrix(ClothModifierData *clmd, ColliderContacts *contacts, int totcolliders, lfVector *X, lfVector *V, fmatrix3x3 *S, lfVector *z, float dt) -{ - Cloth *cloth = clmd->clothObject; - ClothVertex *verts = cloth->verts; - int numverts = cloth->numverts; - RootTransform *roots = cloth->implicit->root; - int i, j, v; - - for (v = 0; v < numverts; v++) { - S[v].c = S[v].r = v; - if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) { - /* pinned vertex constraints */ - zero_v3(z[v]); /* velocity is defined externally */ - zero_m3(S[v].m); - } - else { - /* free vertex */ - zero_v3(z[v]); - unit_m3(S[v].m); - } - - verts[v].impulse_count = 0; - } - - for (i = 0; i < totcolliders; ++i) { - ColliderContacts *ct = &contacts[i]; - for (j = 0; j < ct->totcollisions; ++j) { - CollPair *collpair = &ct->collisions[j]; -// float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - ct->ob->pd->pdef_sbdamp); - float restitution = 0.0f; - int v = collpair->face1; - float cnor[3], cmat[3][3]; - float impulse[3]; - - /* pinned verts handled separately */ - if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) - continue; - - /* calculate collision response */ - if (!collision_response(clmd, ct->collmd, collpair, restitution, impulse)) - continue; - - vel_world_to_root(impulse, X[v], impulse, &roots[v]); - add_v3_v3(z[v], impulse); - ++verts[v].impulse_count; - if (verts[v].impulse_count > 1) - continue; - - /* modify S to enforce velocity constraint in normal direction */ - copy_v3_v3(cnor, collpair->normal); - mul_transposed_m3_v3(roots[v].rot, cnor); - mul_fvectorT_fvector(cmat, cnor, cnor); - - sub_m3_m3m3(S[v].m, I, cmat); - - BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pa, 0, 1, 0, "collision", hash_collpair(936, collpair)); - BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pb, 1, 0, 0, "collision", hash_collpair(937, collpair)); - BKE_sim_debug_data_add_line(clmd->debug_data, collpair->pa, collpair->pb, 0.7, 0.7, 0.7, "collision", hash_collpair(938, collpair)); - - { /* DEBUG */ -// float nor[3]; -// mul_v3_v3fl(nor, collpair->normal, collpair->distance); -// BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, nor, 1, 1, 0, "collision", hash_collpair(939, collpair)); - BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, impulse, 1, 1, 0, "collision", hash_collpair(940, collpair)); -// BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, collpair->normal, 1, 1, 0, "collision", hash_collpair(941, collpair)); - } - } - } -} - -static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVector *lF, lfVector *lX, lfVector *lV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors, float time, fmatrix3x3 *M) -{ - /* Collect forces and derivatives: F, dFdX, dFdV */ - Cloth *cloth = clmd->clothObject; - ClothVertex *verts = cloth->verts; - RootTransform *roots = cloth->implicit->root; - unsigned int i = 0; - float drag = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */ - float gravity[3] = {0.0f, 0.0f, 0.0f}; - MFace *mfaces = cloth->mfaces; - unsigned int numverts = cloth->numverts; - LinkNode *search; - lfVector *winvec; - EffectedPoint epoint; - - /* initialize forces to zero */ - zero_lfvector(lF, numverts); - init_bfmatrix(dFdX, ZERO); - init_bfmatrix(dFdV, ZERO); - -#ifdef CLOTH_FORCE_GRAVITY - /* global acceleration (gravitation) */ - if (clmd->scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) { - copy_v3_v3(gravity, clmd->scene->physics_settings.gravity); - mul_fvector_S(gravity, gravity, 0.001f * clmd->sim_parms->effector_weights->global_gravity); /* scale gravity force */ - } - /* multiply lF with mass matrix - * force = mass * acceleration (in this case: gravity) - */ - for (i = 0; i < numverts; i++) { - float g[3]; - acc_world_to_root(g, lX[i], lV[i], gravity, &roots[i]); - mul_m3_v3(M[i].m, g); - add_v3_v3(lF[i], g); - } -#else - zero_lfvector(lF, numverts); -#endif - - hair_volume_forces(clmd, lF, lX, lV, numverts); - -#ifdef CLOTH_FORCE_DRAG - /* set dFdX jacobi matrix diagonal entries to -spring_air */ - for (i = 0; i < numverts; i++) { - dFdV[i].m[0][0] -= drag; - dFdV[i].m[1][1] -= drag; - dFdV[i].m[2][2] -= drag; - } - submul_lfvectorS(lF, lV, drag, numverts); - for (i = 0; i < numverts; i++) { -#if 1 - float tmp[3][3]; - - /* NB: uses root space velocity, no need to transform */ - madd_v3_v3fl(lF[i], lV[i], -drag); - - copy_m3_m3(tmp, I); - mul_m3_fl(tmp, -drag); - add_m3_m3m3(dFdV[i].m, dFdV[i].m, tmp); -#else - float f[3], tmp[3][3], drag_dfdv[3][3], t[3]; - - mul_v3_v3fl(f, lV[i], -drag); - force_world_to_root(t, lX[i], lV[i], f, verts[i].mass, &roots[i]); - add_v3_v3(lF[i], t); - - copy_m3_m3(drag_dfdv, I); - mul_m3_fl(drag_dfdv, -drag); - dfdv_world_to_root(tmp, drag_dfdv, verts[i].mass, &roots[i]); - add_m3_m3m3(dFdV[i].m, dFdV[i].m, tmp); -#endif - } -#endif - - /* handle external forces like wind */ - if (effectors) { - // 0 = force, 1 = normalized force - winvec = create_lfvector(cloth->numverts); - - if (!winvec) - printf("winvec: out of memory in implicit.c\n"); - - // precalculate wind forces - for (i = 0; i < cloth->numverts; i++) { - pd_point_from_loc(clmd->scene, (float*)lX[i], (float*)lV[i], i, &epoint); - pdDoEffectors(effectors, NULL, clmd->sim_parms->effector_weights, &epoint, winvec[i], NULL); - } - - for (i = 0; i < cloth->numfaces; i++) { - float trinormal[3] = {0, 0, 0}; // normalized triangle normal - float triunnormal[3] = {0, 0, 0}; // not-normalized-triangle normal - float tmp[3] = {0, 0, 0}; - float factor = (mfaces[i].v4) ? 0.25 : 1.0 / 3.0; - factor *= 0.02f; - - // calculate face normal - if (mfaces[i].v4) - CalcFloat4(lX[mfaces[i].v1], lX[mfaces[i].v2], lX[mfaces[i].v3], lX[mfaces[i].v4], triunnormal); - else - CalcFloat(lX[mfaces[i].v1], lX[mfaces[i].v2], lX[mfaces[i].v3], triunnormal); - - normalize_v3_v3(trinormal, triunnormal); - - // add wind from v1 - copy_v3_v3(tmp, trinormal); - mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v1], triunnormal)); - VECADDS(lF[mfaces[i].v1], lF[mfaces[i].v1], tmp, factor); - - // add wind from v2 - copy_v3_v3(tmp, trinormal); - mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v2], triunnormal)); - VECADDS(lF[mfaces[i].v2], lF[mfaces[i].v2], tmp, factor); - - // add wind from v3 - copy_v3_v3(tmp, trinormal); - mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v3], triunnormal)); - VECADDS(lF[mfaces[i].v3], lF[mfaces[i].v3], tmp, factor); - - // add wind from v4 - if (mfaces[i].v4) { - copy_v3_v3(tmp, trinormal); - mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v4], triunnormal)); - VECADDS(lF[mfaces[i].v4], lF[mfaces[i].v4], tmp, factor); - } - } - - /* Hair has only edges */ - if (cloth->numfaces == 0) { - ClothSpring *spring; - float edgevec[3] = {0, 0, 0}; //edge vector - float edgeunnormal[3] = {0, 0, 0}; // not-normalized-edge normal - float tmp[3] = {0, 0, 0}; - float factor = 0.01; - - search = cloth->springs; - while (search) { - spring = search->link; - - if (spring->type == CLOTH_SPRING_TYPE_STRUCTURAL) { - sub_v3_v3v3(edgevec, (float*)lX[spring->ij], (float*)lX[spring->kl]); - - project_v3_v3v3(tmp, winvec[spring->ij], edgevec); - sub_v3_v3v3(edgeunnormal, winvec[spring->ij], tmp); - /* hair doesn't stretch too much so we can use restlen pretty safely */ - VECADDS(lF[spring->ij], lF[spring->ij], edgeunnormal, spring->restlen * factor); - - project_v3_v3v3(tmp, winvec[spring->kl], edgevec); - sub_v3_v3v3(edgeunnormal, winvec[spring->kl], tmp); - VECADDS(lF[spring->kl], lF[spring->kl], edgeunnormal, spring->restlen * factor); - } - - search = search->next; - } - } - - del_lfvector(winvec); - } - - // calculate spring forces - search = cloth->springs; - while (search) { - // only handle active springs - ClothSpring *spring = search->link; - if (!(spring->flags & CLOTH_SPRING_FLAG_DEACTIVATE)) - cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX, time); - - search = search->next; - } - - // apply spring forces - search = cloth->springs; - while (search) { - // only handle active springs - ClothSpring *spring = search->link; - if (!(spring->flags & CLOTH_SPRING_FLAG_DEACTIVATE)) - cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX); - search = search->next; - } - // printf("\n"); -} - -static bool simulate_implicit_euler(Implicit_Data *id, float dt) -{ - unsigned int numverts = id->dFdV[0].vcount; - bool ok; - - lfVector *dFdXmV = create_lfvector(numverts); - zero_lfvector(id->dV, numverts); - - cp_bfmatrix(id->A, id->M); - - subadd_bfmatrixS_bfmatrixS(id->A, id->dFdV, dt, id->dFdX, (dt*dt)); - - mul_bfmatrix_lfvector(dFdXmV, id->dFdX, id->V); - - add_lfvectorS_lfvectorS(id->B, id->F, dt, dFdXmV, (dt*dt), numverts); - - // itstart(); - - ok = cg_filtered(id->dV, id->A, id->B, id->z, id->S); /* conjugate gradient algorithm to solve Ax=b */ - // cg_filtered_pre(id->dV, id->A, id->B, id->z, id->S, id->P, id->Pinv, id->bigI); - - // itend(); - // printf("cg_filtered calc time: %f\n", (float)itval()); - - // advance velocities - add_lfvector_lfvector(id->Vnew, id->V, id->dV, numverts); - - del_lfvector(dFdXmV); - - return ok; -} - -/* computes where the cloth would be if it were subject to perfectly stiff edges - * (edge distance constraints) in a lagrangian solver. then add forces to help - * guide the implicit solver to that state. this function is called after - * collisions*/ -static int UNUSED_FUNCTION(cloth_calc_helper_forces)(Object *UNUSED(ob), ClothModifierData *clmd, float (*initial_cos)[3], float UNUSED(step), float dt) -{ - Cloth *cloth= clmd->clothObject; - float (*cos)[3] = MEM_callocN(sizeof(float)*3*cloth->numverts, "cos cloth_calc_helper_forces"); - float *masses = MEM_callocN(sizeof(float)*cloth->numverts, "cos cloth_calc_helper_forces"); - LinkNode *node; - ClothSpring *spring; - ClothVertex *cv; - int i, steps; - - cv = cloth->verts; - for (i=0; i<cloth->numverts; i++, cv++) { - copy_v3_v3(cos[i], cv->tx); - - if (cv->goal == 1.0f || len_squared_v3v3(initial_cos[i], cv->tx) != 0.0f) { - masses[i] = 1e+10; - } - else { - masses[i] = cv->mass; - } - } - - steps = 55; - for (i=0; i<steps; i++) { - for (node=cloth->springs; node; node=node->next) { - /* ClothVertex *cv1, *cv2; */ /* UNUSED */ - int v1, v2; - float len, c, l, vec[3]; - - spring = node->link; - if (spring->type != CLOTH_SPRING_TYPE_STRUCTURAL && spring->type != CLOTH_SPRING_TYPE_SHEAR) - continue; - - v1 = spring->ij; v2 = spring->kl; - /* cv1 = cloth->verts + v1; */ /* UNUSED */ - /* cv2 = cloth->verts + v2; */ /* UNUSED */ - len = len_v3v3(cos[v1], cos[v2]); - - sub_v3_v3v3(vec, cos[v1], cos[v2]); - normalize_v3(vec); - - c = (len - spring->restlen); - if (c == 0.0f) - continue; - - l = c / ((1.0f / masses[v1]) + (1.0f / masses[v2])); - - mul_v3_fl(vec, -(1.0f / masses[v1]) * l); - add_v3_v3(cos[v1], vec); - - sub_v3_v3v3(vec, cos[v2], cos[v1]); - normalize_v3(vec); - - mul_v3_fl(vec, -(1.0f / masses[v2]) * l); - add_v3_v3(cos[v2], vec); - } - } - - cv = cloth->verts; - for (i=0; i<cloth->numverts; i++, cv++) { - float vec[3]; - - /*compute forces*/ - sub_v3_v3v3(vec, cos[i], cv->tx); - mul_v3_fl(vec, cv->mass*dt*20.0f); - add_v3_v3(cv->tv, vec); - //copy_v3_v3(cv->tx, cos[i]); - } - - MEM_freeN(cos); - MEM_freeN(masses); - - return 1; -} - -int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors) -{ - unsigned int i=0; - float step=0.0f, tf=clmd->sim_parms->timescale; - Cloth *cloth = clmd->clothObject; - ClothVertex *verts = cloth->verts/*, *cv*/; - unsigned int numverts = cloth->numverts; - float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame; - float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; - /*float (*initial_cos)[3] = MEM_callocN(sizeof(float)*3*cloth->numverts, "initial_cos implicit.c");*/ /* UNUSED */ - Implicit_Data *id = cloth->implicit; - ColliderContacts *contacts = NULL; - int totcolliders = 0; - - BKE_sim_debug_data_clear_category(clmd->debug_data, "collision"); - - if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { /* do goal stuff */ - for (i = 0; i < numverts; i++) { - // update velocities with constrained velocities from pinned verts - if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) { - float v[3]; - sub_v3_v3v3(v, verts[i].xconst, verts[i].xold); - // mul_v3_fl(id->V[i], clmd->sim_parms->stepsPerFrame); - vel_world_to_root(id->V[i], id->X[i], v, &id->root[i]); - } - } - } - - if (clmd->debug_data) { - for (i = 0; i < numverts; i++) { - BKE_sim_debug_data_add_dot(clmd->debug_data, verts[i].x, 1.0f, 0.1f, 1.0f, "points", hash_vertex(583, i)); - } - } - - while (step < tf) { - - /* copy velocities for collision */ - for (i = 0; i < numverts; i++) { - vel_root_to_world(verts[i].tv, id->X[i], id->V[i], &id->root[i]); - copy_v3_v3(verts[i].v, verts[i].tv); - } - - /* determine contact points */ - if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) { - if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_POINTS) { - cloth_find_point_contacts(ob, clmd, 0.0f, tf, &contacts, &totcolliders); - } - } - - /* setup vertex constraints for pinned vertices and contacts */ - setup_constraint_matrix(clmd, contacts, totcolliders, id->X, id->V, id->S, id->z, dt); - - // damping velocity for artistic reasons - mul_lfvectorS(id->V, id->V, clmd->sim_parms->vel_damping, numverts); - - // calculate forces - cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M); - - // calculate new velocity - simulate_implicit_euler(id, dt); - - // advance positions - add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts); - - /* move pinned verts to correct position */ - for (i = 0; i < numverts; i++) { - if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { - if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) { - float x[3]; - interp_v3_v3v3(x, verts[i].xold, verts[i].xconst, step + dt); - loc_world_to_root(id->Xnew[i], x, &id->root[i]); - } - } - - loc_root_to_world(verts[i].txold, id->X[i], &id->root[i]); - - if (!(verts[i].flags & CLOTH_VERT_FLAG_PINNED) && i > 0) { - BKE_sim_debug_data_add_line(clmd->debug_data, id->X[i], id->X[i-1], 0.6, 0.3, 0.3, "hair", hash_vertex(4892, i)); - BKE_sim_debug_data_add_line(clmd->debug_data, id->Xnew[i], id->Xnew[i-1], 1, 0.5, 0.5, "hair", hash_vertex(4893, i)); - } - BKE_sim_debug_data_add_vector(clmd->debug_data, id->X[i], id->V[i], 0, 0, 1, "velocity", hash_vertex(3158, i)); - } - - /* free contact points */ - if (contacts) { - cloth_free_contacts(contacts, totcolliders); - } - -#if 0 - if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) { - bool do_extra_solve = false; - - // collisions - // itstart(); - - // update verts to current positions - for (i = 0; i < numverts; i++) { - copy_v3_v3(verts[i].tx, id->Xnew[i]); - - sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold); - copy_v3_v3(verts[i].v, verts[i].tv); - } - - /* unused */ - /*for (i=0, cv=cloth->verts; i<cloth->numverts; i++, cv++) { - copy_v3_v3(initial_cos[i], cv->tx); - }*/ - - if (clmd->clothObject->bvhtree) { - // call collision function - // TODO: check if "step" or "step+dt" is correct - dg - do_extra_solve = cloth_bvh_objcollision(ob, clmd, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale); - } - else if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_POINTS) { - do_extra_solve = cloth_points_objcollision(ob, clmd, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale); - } - - // copy corrected positions back to simulation - for (i = 0; i < numverts; i++) { - // correct velocity again, just to be sure we had to change it due to adaptive collisions - sub_v3_v3v3(verts[i].tv, verts[i].tx, id->X[i]); - } - - /* unused */ - /*if (do_extra_solve) - cloth_calc_helper_forces(ob, clmd, initial_cos, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale);*/ - - if (do_extra_solve) { - for (i = 0; i < numverts; i++) { - if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED)) - continue; - - copy_v3_v3(id->Xnew[i], verts[i].tx); - copy_v3_v3(id->Vnew[i], verts[i].tv); - mul_v3_fl(id->Vnew[i], spf); - } - } - - // X = Xnew; - cp_lfvector(id->X, id->Xnew, numverts); - - // if there were collisions, advance the velocity from v_n+1/2 to v_n+1 - - if (do_extra_solve) { - // V = Vnew; - cp_lfvector(id->V, id->Vnew, numverts); - - // calculate - cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step+dt, id->M); - - simulate_implicit_euler(id, dt / 2.0f); - } - } - else { - // X = Xnew; - cp_lfvector(id->X, id->Xnew, numverts); - } - - // itend(); - // printf("collision time: %f\n", (float)itval()); -#else - // X = Xnew; - cp_lfvector(id->X, id->Xnew, numverts); -#endif - - // V = Vnew; - cp_lfvector(id->V, id->Vnew, numverts); - - step += dt; - } - - for (i = 0; i < numverts; i++) { - if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED)) { - copy_v3_v3(verts[i].txold, verts[i].xconst); // TODO: test --> should be .x - copy_v3_v3(verts[i].x, verts[i].xconst); - - vel_root_to_world(verts[i].v, id->X[i], id->V[i], &id->root[i]); - } - else { - loc_root_to_world(verts[i].txold, id->X[i], &id->root[i]); - copy_v3_v3(verts[i].x, verts[i].txold); - - vel_root_to_world(verts[i].v, id->X[i], id->V[i], &id->root[i]); - } - } - - /* unused */ - /*MEM_freeN(initial_cos);*/ - - return 1; -} - -void implicit_set_positions(ClothModifierData *clmd) -{ - Cloth *cloth = clmd->clothObject; - ClothVertex *verts = cloth->verts; - unsigned int numverts = cloth->numverts, i; - ClothHairRoot *cloth_roots = clmd->roots; - Implicit_Data *id = cloth->implicit; - - for (i = 0; i < numverts; i++) { -#ifdef CLOTH_ROOT_FRAME - copy_v3_v3(id->root[i].loc, cloth_roots[i].loc); - copy_m3_m3(id->root[i].rot, cloth_roots[i].rot); -#else - zero_v3(id->root[i].loc); - unit_m3(id->root[i].rot); - (void)cloth_roots; -#endif - - loc_world_to_root(id->X[i], verts[i].x, &id->root[i]); - vel_world_to_root(id->V[i], id->X[i], verts[i].v, &id->root[i]); - } - if (G.debug_value > 0) - printf("implicit_set_positions\n"); -} - -#endif /* IMPLICIT_SOLVER_BLENDER */ |