Patch #34204: [Render Animation] Fails with "Error: Specified sample_fmt is not suppo...
[blender.git] / source / blender / blenkernel / intern / implicit.c
index 4aef471..b9064fe 100644 (file)
 #include "BKE_global.h"
 
 
-#define CLOTH_OPENMP_LIMIT 512
+#ifdef _OPENMP
+#  define CLOTH_OPENMP_LIMIT 512
+#endif
 
+#if 0  /* debug timing */
 #ifdef _WIN32
 #include <windows.h>
 static LARGE_INTEGER _itstart, _itend;
@@ -81,7 +84,7 @@ double itval(void)
 
 static struct timeval _itstart, _itend;
 static struct timezone itz;
-void itstart(void)
+static void itstart(void)
 {
        gettimeofday(&_itstart, &itz);
 }
@@ -89,7 +92,7 @@ static void itend(void)
 {
        gettimeofday(&_itend, &itz);
 }
-double itval(void)
+static double itval(void)
 {
        double t1, t2;
        t1 =  (double)_itstart.tv_sec + (double)_itstart.tv_usec/(1000*1000);
@@ -97,6 +100,7 @@ double itval(void)
        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}};
@@ -120,7 +124,7 @@ 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? */
+       /* 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 */ 
@@ -185,15 +189,15 @@ DO_INLINE void print_lfvector(float (*fLongVector)[3], unsigned int verts)
 /* create long vector */
 DO_INLINE lfVector *create_lfvector(unsigned int verts)
 {
-       // TODO: check if memory allocation was successfull */
-       return  (lfVector *)MEM_callocN (verts * sizeof(lfVector), "cloth_implicit_alloc_vector");
+       /* 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);
+               MEM_freeN(fLongVector);
                // cloth_aligned_free(&MEMORY_BASE, fLongVector);
        }
 }
@@ -513,8 +517,8 @@ static void print_bfmatrix(fmatrix3x3 *m3)
 /* create big matrix */
 DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs)
 {
-       // TODO: check if memory allocation was successfull */
-       fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN (sizeof (fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix");
+       // 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;
@@ -523,14 +527,14 @@ DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs)
 DO_INLINE void del_bfmatrix(fmatrix3x3 *matrix)
 {
        if (matrix != NULL) {
-               MEM_freeN (matrix);
+               MEM_freeN(matrix);
        }
 }
 
 /* copy big matrix */
 DO_INLINE void cp_bfmatrix(fmatrix3x3 *to, fmatrix3x3 *from)
-{      
-       // TODO bounds checking 
+{
+       // TODO bounds checking
        memcpy(to, from, sizeof(fmatrix3x3) * (from[0].vcount+from[0].scount));
 }
 
@@ -586,7 +590,7 @@ DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector
                        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++) {
@@ -619,7 +623,7 @@ DO_INLINE void add_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix
 
        /* process diagonal elements */
        for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) {
-               add_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
+               add_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);
        }
 
 }
@@ -630,7 +634,7 @@ DO_INLINE void addadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmat
 
        /* process diagonal elements */
        for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) {
-               addadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
+               addadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);
        }
 
 }
@@ -641,7 +645,7 @@ DO_INLINE void subadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmat
 
        /* process diagonal elements */
        for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) {
-               subadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
+               subadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);
        }
 
 }
@@ -652,7 +656,7 @@ DO_INLINE void sub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix
 
        /* process diagonal elements */
        for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) {
-               sub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
+               sub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);
        }
 
 }
@@ -663,7 +667,7 @@ DO_INLINE void sub_bfmatrix_Smatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3
 
        /* process diagonal elements */
        for (i = 0; i < matrix[0].vcount; i++) {
-               sub_fmatrix_fmatrix(to[matrix[i].c].m, from[matrix[i].c].m, matrix[i].m);       
+               sub_fmatrix_fmatrix(to[matrix[i].c].m, from[matrix[i].c].m, matrix[i].m);
        }
 
 }
@@ -674,7 +678,7 @@ DO_INLINE void addsub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmat
 
        /* process diagonal elements */
        for (i = 0; i < matrix[0].vcount+matrix[0].scount; i++) {
-               addsub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
+               addsub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);
        }
 
 }
@@ -687,7 +691,7 @@ DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, flo
 
        /* 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);      
+               subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS);
        }
 
 }
@@ -700,17 +704,39 @@ typedef struct Implicit_Data  {
        fmatrix3x3 *A, *dFdV, *dFdX, *S, *P, *Pinv, *bigI, *M; 
 } Implicit_Data;
 
+/* Init constraint matrix */
+static void update_matrixS(ClothVertex *verts, int numverts, fmatrix3x3 *S)
+{
+       unsigned int pinned = 0;
+       int i = 0;
+
+       /* Clear matrix from old vertex constraints */
+       for (i = 0; i < S[0].vcount; i++)
+               S[i].c = S[i].r = 0;
+
+       /* Set new vertex constraints */
+       for (i = 0; i < numverts; i++) {
+               if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
+                       S[pinned].c = S[pinned].r = i;
+                       pinned++;
+               }
+       }
+
+       // S is special and needs specific vcount and scount
+       S[0].vcount = pinned; 
+       S[0].scount = 0;
+}
+
 int implicit_init(Object *UNUSED(ob), ClothModifierData *clmd)
 {
        unsigned int i = 0;
-       unsigned int pinned = 0;
        Cloth *cloth = NULL;
        ClothVertex *verts = NULL;
        ClothSpring *spring = NULL;
        Implicit_Data *id = NULL;
        LinkNode *search = NULL;
        
-       if (G.rt > 0)
+       if (G.debug_value > 0)
                printf("implicit_init\n");
 
        // init memory guard
@@ -720,10 +746,10 @@ int implicit_init(Object *UNUSED(ob), ClothModifierData *clmd)
        verts = cloth->verts;
 
        // create implicit base
-       id = (Implicit_Data *)MEM_callocN (sizeof(Implicit_Data), "implicit vecmat");
+       id = (Implicit_Data *)MEM_callocN(sizeof(Implicit_Data), "implicit vecmat");
        cloth->implicit = id;
 
-       /* process diagonal elements */         
+       /* 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);
@@ -743,24 +769,19 @@ int implicit_init(Object *UNUSED(ob), ClothModifierData *clmd)
        id->dV = create_lfvector(cloth->numverts);
        id->z = create_lfvector(cloth->numverts);
        
-       for (i=0;i<cloth->numverts;i++) {
+       id->S[0].vcount = 0;
+
+       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;
 
-               if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
-                       id->S[pinned].pinned = 1;
-                       id->S[pinned].c = id->S[pinned].r = i;
-                       pinned++;
-               }
+               update_matrixS(verts, cloth->numverts, id->S);
                
                initdiag_fmatrixS(id->M[i].m, verts[i].mass);
        }
 
-       // S is special and needs specific vcount and scount
-       id->S[0].vcount = pinned; id->S[0].scount = 0;
-
        // init springs 
        search = cloth->springs;
-       for (i=0;i<cloth->numsprings;i++) {
+       for (i = 0; i < cloth->numsprings; i++) {
                spring = search->link;
                
                // dFdV_start[i].r = big_I[i].r = big_zero[i].r = 
@@ -784,6 +805,7 @@ int implicit_init(Object *UNUSED(ob), ClothModifierData *clmd)
 
        return 1;
 }
+
 int    implicit_free(ClothModifierData *clmd)
 {
        Implicit_Data *id;
@@ -822,41 +844,40 @@ int       implicit_free(ClothModifierData *clmd)
 
 DO_INLINE float fb(float length, float L)
 {
-       float x = length/L;
-       return (-11.541f*pow(x, 4)+34.193f*pow(x, 3)-39.083f*pow(x, 2)+23.116f*x-9.713f);
+       float x = length / L;
+       return (-11.541f * powf(x, 4) + 34.193f * powf(x, 3) - 39.083f * powf(x, 2) + 23.116f * x - 9.713f);
 }
 
 DO_INLINE float fbderiv(float length, float L)
 {
        float x = length/L;
 
-       return (-46.164f*pow(x, 3)+102.579f*pow(x, 2)-78.166f*x+23.116f);
+       return (-46.164f * powf(x, 3) + 102.579f * powf(x, 2) - 78.166f * x + 23.116f);
 }
 
 DO_INLINE float fbstar(float length, float L, float kb, float cb)
 {
-       float tempfb = kb * fb(length, L);
-
-       float fbstar = cb * (length - L);
+       float tempfb_fl = kb * fb(length, L);
+       float fbstar_fl = cb * (length - L);
        
-       if (tempfb < fbstar)
-               return fbstar;
+       if (tempfb_fl < fbstar_fl)
+               return fbstar_fl;
        else
-               return tempfb;          
+               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 = kb * fb(length, L);
-       float fbstar = cb * (length - L);
+       float tempfb_fl = kb * fb(length, L);
+       float fbstar_fl = cb * (length - L);
 
-       if (tempfb < fbstar) {
+       if (tempfb_fl < fbstar_fl) {
                return cb;
        }
        else {
-               return kb * fbderiv(length, L); 
-       }       
+               return kb * fbderiv(length, L);
+       }
 }
 
 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
@@ -896,7 +917,7 @@ static int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z
        cp_lfvector(d, r, numverts);
 
        s = dot_lfvector(r, r, numverts);
-       starget = s * sqrt(conjgrad_epsilon);
+       starget = s * sqrtf(conjgrad_epsilon);
 
        while (s>starget && conjgrad_loopcount < conjgrad_looplimit) {
                // Mul(q, A, d); // q = A*d;
@@ -1127,9 +1148,9 @@ DO_INLINE void dfdx_spring_type1(float to[3][3], float extent[3], float length,
        // dir is unit length direction, rest is spring's restlength, k is spring constant.
        // return  (outerprod(dir, dir)*k + (I - outerprod(dir, dir))*(k - ((k*L)/length)));
        float temp[3][3];
-       float temp1 = k*(1.0 - (L/length));     
+       float temp1 = k * (1.0f - (L / length));
        
-       mul_fvectorT_fvectorS(temp, extent, extent, 1.0 / dot);
+       mul_fvectorT_fvectorS(temp, extent, extent, 1.0f / dot);
        sub_fmatrix_fmatrix(to, I, temp);
        mul_fmatrix_S(to, temp1);
        
@@ -1197,7 +1218,7 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
        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 nulldfdx[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
        
        float scaling = 0.0;
 
@@ -1225,7 +1246,7 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                s->flags |= CSPRING_FLAG_DEACTIVATE;
                return;
        }
-       } 
+       }
                */
                mul_fvector_S(dir, extent, 1.0f/length);
        }
@@ -1239,13 +1260,13 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                        s->flags |= CLOTH_SPRING_FLAG_NEEDED;
                        
                        k = clmd->sim_parms->structural;
-                               
+
                        scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
-                       
+
                        k = scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
-                       
+
                        // TODO: verify, half verified (couldn't see error)
-                       mul_fvector_S(stretch_force, dir, k*(length-L)); 
+                       mul_fvector_S(stretch_force, dir, k*(length-L));
 
                        VECADD(s->f, s->f, stretch_force);
 
@@ -1286,7 +1307,7 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                
                VECADDS(s->f, s->f, extent, -k);
                
-               mul_fvector_S(damping_force, dir, clmd->sim_parms->goalfrict * 0.01 * dot_v3v3(vel, dir));
+               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!!!!
@@ -1297,10 +1318,10 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                if (length < L) {
                        s->flags |= CLOTH_SPRING_FLAG_NEEDED;
                        
-                       k = clmd->sim_parms->bending;   
+                       k = clmd->sim_parms->bending;
                        
-                       scaling = k + s->stiffness * ABS(clmd->sim_parms->max_bend-k);                  
-                       cb = k = scaling / (20.0*(clmd->sim_parms->avg_spring_len + FLT_EPSILON));
+                       scaling = k + s->stiffness * ABS(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);
@@ -1316,7 +1337,7 @@ DO_INLINE void cloth_apply_spring_force(ClothModifierData *UNUSED(clmd), ClothSp
                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); 
+                       add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv);
                }
 
                VECADD(lF[s->ij], lF[s->ij], s->f);
@@ -1327,7 +1348,7 @@ DO_INLINE void cloth_apply_spring_force(ClothModifierData *UNUSED(clmd), ClothSp
                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);
-       }       
+       }
 }
 
 
@@ -1335,15 +1356,15 @@ 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];
+       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)
@@ -1351,17 +1372,17 @@ 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];
+       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];
+       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];
+       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(float wind[3], float vertexnormal[3])  
@@ -1428,7 +1449,7 @@ static void hair_velocity_smoothing(ClothModifierData *clmd, lfVector *lF, lfVec
                i = HAIR_GRID_INDEX(lX[v], gmin, gmax, 0);
                j = HAIR_GRID_INDEX(lX[v], gmin, gmax, 1);
                k = HAIR_GRID_INDEX(lX[v], gmin, gmax, 2);
-               if (i < 0 || j < 0 || k < 0 || i > 10 || j >= 10 || k >= 10)
+               if (i < 0 || j < 0 || k < 0 || i >= 10 || j >= 10 || k >= 10)
                        continue;
 
                grid[i][j][k].velocity[0] += lV[v][0];
@@ -1458,7 +1479,7 @@ static void hair_velocity_smoothing(ClothModifierData *clmd, lfVector *lF, lfVec
                                                colg[i][j][k].velocity[0] += vel[0];
                                                colg[i][j][k].velocity[1] += vel[1];
                                                colg[i][j][k].velocity[2] += vel[2];
-                                               colg[i][j][k].density += 1.0;
+                                               colg[i][j][k].density += 1.0f;
                                        }
                                }
                        }
@@ -1492,7 +1513,7 @@ static void hair_velocity_smoothing(ClothModifierData *clmd, lfVector *lF, lfVec
                i = HAIR_GRID_INDEX(lX[v], gmin, gmax, 0);
                j = HAIR_GRID_INDEX(lX[v], gmin, gmax, 1);
                k = HAIR_GRID_INDEX(lX[v], gmin, gmax, 2);
-               if (i < 0 || j < 0 || k < 0 || i > 10 || j >= 10 || k >= 10)
+               if (i < 0 || j < 0 || k < 0 || i >= 10 || j >= 10 || k >= 10)
                        continue;
 
                lF[v][0] += smoothfac * (grid[i][j][k].velocity[0] - lV[v][0]);
@@ -1523,7 +1544,7 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec
        lfVector *winvec;
        EffectedPoint epoint;
 
-       tm2[0][0]= tm2[1][1]= tm2[2][2]= -spring_air;
+       tm2[0][0] = tm2[1][1] = tm2[2][2] = -spring_air;
        
        /* global acceleration (gravitation) */
        if (clmd->scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
@@ -1567,11 +1588,11 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec
                }
                
                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 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.02;
+                       factor *= 0.02f;
                        
                        // calculate face normal
                        if (mfaces[i].v4)
@@ -1607,9 +1628,9 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec
                /* 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 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;
@@ -1640,8 +1661,9 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec
        search = cloth->springs;
        while (search) {
                // only handle active springs
-               // if (((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED)) {}
-               cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX, time);
+               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;
        }
@@ -1650,8 +1672,9 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec
        search = cloth->springs;
        while (search) {
                // only handle active springs
-               // if (((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED)) 
-               cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX);
+               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");
@@ -1671,13 +1694,13 @@ static void simulate_implicit_euler(lfVector *Vnew, lfVector *UNUSED(lX), lfVect
        mul_bfmatrix_lfvector(dFdXmV, dFdX, lV);
 
        add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, (dt*dt), numverts);
-       
-       itstart();
-       
+
+       // itstart();
+
        cg_filtered(dV, A, B, z, S); /* conjugate gradient algorithm to solve Ax=b */
        // cg_filtered_pre(dV, A, B, z, S, P, Pinv, bigI);
-       
-       itend();
+
+       // itend();
        // printf("cg_filtered calc time: %f\n", (float)itval());
        
        cp_lfvector(olddV, dV, numverts);
@@ -1693,7 +1716,7 @@ static void simulate_implicit_euler(lfVector *Vnew, lfVector *UNUSED(lX), lfVect
  * (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*/
-int cloth_calc_helper_forces(Object *UNUSED(ob), ClothModifierData * clmd, float (*initial_cos)[3], float UNUSED(step), float dt)
+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");
@@ -1707,8 +1730,8 @@ int cloth_calc_helper_forces(Object *UNUSED(ob), ClothModifierData * clmd, float
        for (i=0; i<cloth->numverts; i++, cv++) {
                copy_v3_v3(cos[i], cv->tx);
                
-               if (cv->goal == 1.0f || len_v3v3(initial_cos[i], cv->tx) != 0.0) {
-                       masses[i] = 1e+10;      
+               if (cv->goal == 1.0f || len_squared_v3v3(initial_cos[i], cv->tx) != 0.0f) {
+                       masses[i] = 1e+10;
                }
                else {
                        masses[i] = cv->mass;
@@ -1735,18 +1758,18 @@ int cloth_calc_helper_forces(Object *UNUSED(ob), ClothModifierData * clmd, float
                        normalize_v3(vec);
                        
                        c = (len - spring->restlen);
-                       if (c == 0.0)
+                       if (c == 0.0f)
                                continue;
                        
-                       l = c / ((1.0/masses[v1]) + (1.0/masses[v2]));
+                       l = c / ((1.0f / masses[v1]) + (1.0f / masses[v2]));
                        
-                       mul_v3_fl(vec, -(1.0/masses[v1])*l);
+                       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.0/masses[v2])*l);
+                       mul_v3_fl(vec, -(1.0f / masses[v2]) * l);
                        add_v3_v3(cos[v2], vec);
                }
        }
@@ -1781,13 +1804,17 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
        int do_extra_solve;
 
        if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { /* do goal stuff */
+               
+               /* Update vertex constraints for pinned vertices */
+               update_matrixS(verts, cloth->numverts, id->S);
+
                for (i = 0; i < numverts; i++) {
                        // update velocities with constrained velocities from pinned verts
                        if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
                                sub_v3_v3v3(id->V[i], verts[i].xconst, verts[i].xold);
                                // mul_v3_fl(id->V[i], clmd->sim_parms->stepsPerFrame);
                        }
-               }       
+               }
        }
        
        while (step < tf) {
@@ -1812,7 +1839,7 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
                                        mul_fvector_S(tvect, tvect, step+dt);
                                        VECADD(tvect, tvect, verts[i].xold);
                                        copy_v3_v3(id->Xnew[i], tvect);
-                               }       
+                               }
                        }
                        
                        copy_v3_v3(verts[i].txold, id->X[i]);
@@ -1833,11 +1860,11 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
                        for (i=0, cv=cloth->verts; i<cloth->numverts; i++, cv++) {
                                copy_v3_v3(initial_cos[i], cv->tx);
                        }
-                       
+
                        // 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);
-                                               
+
                        // 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
@@ -1870,7 +1897,7 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
                                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);      
+                               cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step+dt, id->M);
                                
                                simulate_implicit_euler(id->Vnew, id->X, id->V, id->F, id->dFdV, id->dFdX, dt / 2.0f, id->A, id->B, id->dV, id->S, id->z, id->olddV, id->P, id->Pinv, id->M, id->bigI);
                        }
@@ -1918,7 +1945,7 @@ void implicit_set_positions(ClothModifierData *clmd)
                copy_v3_v3(id->X[i], verts[i].x);
                copy_v3_v3(id->V[i], verts[i].v);
        }
-       if (G.rt > 0)
-               printf("implicit_set_positions\n");     
+       if (G.debug_value > 0)
+               printf("implicit_set_positions\n");
 }