svn merge -r 15392:15551 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / source / blender / blenkernel / intern / implicit.c
index 8490966283ef893643b505af37f0e2ee81382d57..297ac0b1530c874aa0434b6b6e7840524ab4fb7e 100644 (file)
@@ -1,15 +1,12 @@
 /*  implicit.c      
 * 
 *
 /*  implicit.c      
 * 
 *
-* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
+* ***** 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
 *
 * 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. The Blender
-* Foundation also sells licenses for use in proprietary software under
-* the Blender License.  See http://www.blender.org/BL/ for information
-* about this.
+* 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
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 *
 * Contributor(s): none yet.
 *
 *
 * Contributor(s): none yet.
 *
-* ***** END GPL/BL DUAL LICENSE BLOCK *****
+* ***** END GPL LICENSE BLOCK *****
 */
 */
-#include "math.h"
-#include "float.h"
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
+
 #include "MEM_guardedalloc.h"
 #include "MEM_guardedalloc.h"
-/* types */
-#include "DNA_curve_types.h"
-#include "DNA_object_types.h"
-#include "DNA_object_force.h"  
+
+#include "BKE_cloth.h"
+
 #include "DNA_cloth_types.h"   
 #include "DNA_cloth_types.h"   
-#include "DNA_key_types.h"
-#include "DNA_mesh_types.h"
-#include "DNA_modifier_types.h"
-#include "DNA_meshdata_types.h"
-#include "DNA_lattice_types.h"
 #include "DNA_scene_types.h"
 #include "DNA_scene_types.h"
-#include "DNA_modifier_types.h"
-#include "BLI_blenlib.h"
-#include "BLI_arithb.h"
-#include "BLI_threads.h"
-#include "BKE_curve.h"
-#include "BKE_displist.h"
+
 #include "BKE_effect.h"
 #include "BKE_global.h"
 #include "BKE_effect.h"
 #include "BKE_global.h"
-#include "BKE_key.h"
-#include "BKE_object.h"
 #include "BKE_cloth.h"
 #include "BKE_cloth.h"
-#include "BKE_modifier.h"
 #include "BKE_utildefines.h"
 #include "BKE_utildefines.h"
-#include "BKE_global.h"
-#include  "BIF_editdeform.h"
-
 
 #ifdef _WIN32
 #include <windows.h>
 
 #ifdef _WIN32
 #include <windows.h>
@@ -111,9 +87,6 @@ double itval()
 }
 #endif
 
 }
 #endif
 
-/* callbacks for errors and interrupts and some goo */
-static int (*CT_localInterruptCallBack)(void) = NULL;
-
 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}};
 
 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}};
 
@@ -172,8 +145,9 @@ DO_INLINE void mul_fvectorT_fvector(float to[3][3], float vectorA[3], float vect
 /* 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)
 /* 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_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);
        mul_fvector_S(to[0], to[0], aS);
        mul_fvector_S(to[1], to[1], aS);
        mul_fvector_S(to[2], to[2], aS);
@@ -256,11 +230,11 @@ DO_INLINE void submul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float s
 /* dot product for big vector */
 DO_INLINE float dot_lfvector(float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
 {
 /* dot product for big vector */
 DO_INLINE float dot_lfvector(float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
 {
-       unsigned int i = 0;
+       long i = 0;
        float temp = 0.0;
 // schedule(guided, 2)
 #pragma omp parallel for reduction(+: temp)
        float temp = 0.0;
 // schedule(guided, 2)
 #pragma omp parallel for reduction(+: temp)
-       for(i = 0; i < verts; i++)
+       for(i = 0; i < (long)verts; i++)
        {
                temp += INPR(fLongVectorA[i], fLongVectorB[i]);
        }
        {
                temp += INPR(fLongVectorA[i], fLongVectorB[i]);
        }
@@ -783,7 +757,7 @@ int implicit_init (Object *ob, ClothModifierData *clmd)
        {
                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;
 
        {
                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].goal >= SOFTGOALSNAP)
+               if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
                {
                        id->S[pinned].pinned = 1;
                        id->S[pinned].c = id->S[pinned].r = i;
                {
                        id->S[pinned].pinned = 1;
                        id->S[pinned].c = id->S[pinned].r = i;
@@ -814,6 +788,8 @@ int implicit_init (Object *ob, ClothModifierData *clmd)
                
                search = search->next;
        }
                
                search = search->next;
        }
+       
+       initdiag_bfmatrix(id->bigI, I);
 
        for(i = 0; i < cloth->numverts; i++)
        {               
 
        for(i = 0; i < cloth->numverts; i++)
        {               
@@ -878,7 +854,7 @@ 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 = kb * fb(length, L);
 
        float fbstar = cb * (length - L);
-
+       
        if(tempfb < fbstar)
                return fbstar;
        else
        if(tempfb < fbstar)
                return fbstar;
        else
@@ -992,7 +968,7 @@ DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv)
                
        }
 }
                
        }
 }
-
+/*
 // version 1.3
 int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
 {
 // version 1.3
 int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
 {
@@ -1056,6 +1032,109 @@ int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fma
        del_lfvector(p);
        del_lfvector(r);
        
        del_lfvector(p);
        del_lfvector(r);
        
+       printf("iterations: %d\n", iterations);
+       
+       return iterations<conjgrad_looplimit;
+}
+*/
+// version 1.4
+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;
        // printf("iterations: %d\n", iterations);
        
        return iterations<conjgrad_looplimit;
@@ -1068,11 +1147,14 @@ DO_INLINE void dfdx_spring_type1(float to[3][3], float extent[3], float length,
        // return  (outerprod(dir,dir)*k + (I - outerprod(dir,dir))*(k - ((k*L)/length)));
        float temp[3][3];
        float temp1 = k*(1.0 - (L/length));     
        // return  (outerprod(dir,dir)*k + (I - outerprod(dir,dir))*(k - ((k*L)/length)));
        float temp[3][3];
        float temp1 = k*(1.0 - (L/length));     
+       
        mul_fvectorT_fvectorS(temp, extent, extent, 1.0 / dot);
        sub_fmatrix_fmatrix(to, I, temp);
        mul_fmatrix_S(to, temp1);
        mul_fvectorT_fvectorS(temp, extent, extent, 1.0 / dot);
        sub_fmatrix_fmatrix(to, I, temp);
        mul_fmatrix_S(to, temp1);
+       
        mul_fvectorT_fvectorS(temp, extent, extent, k/ dot);
        add_fmatrix_fmatrix(to, to, temp);
        mul_fvectorT_fvectorS(temp, extent, extent, k/ dot);
        add_fmatrix_fmatrix(to, to, temp);
+       
        /*
        mul_fvectorT_fvector(temp, dir, dir);
        sub_fmatrix_fmatrix(to, I, temp);
        /*
        mul_fvectorT_fvector(temp, dir, dir);
        sub_fmatrix_fmatrix(to, I, temp);
@@ -1088,10 +1170,10 @@ DO_INLINE void dfdx_spring_type2(float to[3][3], float dir[3], float length, flo
        mul_fvectorT_fvectorS(to, 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 ext[3], float damping, float dot)
+DO_INLINE void dfdv_damp(float to[3][3], float dir[3], float damping)
 {
        // derivative of force wrt velocity.  
 {
        // derivative of force wrt velocity.  
-       mul_fvectorT_fvectorS(to, ext, ext, damping / dot);
+       mul_fvectorT_fvectorS(to, dir, dir, damping);
        
 }
 
        
 }
 
@@ -1117,8 +1199,10 @@ DO_INLINE void dfdx_damp(float to[3][3],  float dir[3],float length,const float
 
 }
 
 
 }
 
-DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
+DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *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 extent[3];
        float length = 0, dot = 0;
        float dir[3] = {0,0,0};
@@ -1147,7 +1231,7 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
        
        s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
        
        
        s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
        
-       if(length > ABS(ALMOST_ZERO))
+       if(length > ALMOST_ZERO)
        {
                /*
                if(length>L)
        {
                /*
                if(length>L)
@@ -1168,7 +1252,7 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
        }
        
        // calculate force of structural + shear springs
        }
        
        // calculate force of structural + shear springs
-       if(s->type != CLOTH_SPRING_TYPE_BENDING)
+       if((s->type & CLOTH_SPRING_TYPE_STRUCTURAL) || (s->type & CLOTH_SPRING_TYPE_SHEAR))
        {
                if(length > L) // only on elonglation
                {
        {
                if(length > L) // only on elonglation
                {
@@ -1178,18 +1262,7 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                                
                        scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
                        
                                
                        scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
                        
-                       k = scaling / (clmd->coll_parms->avg_spring_len + FLT_EPSILON);
-                       if(G.rt>3)
-                       printf("struct scaling: %f\n", k);
-                       
-                       /*
-                       if((s->ij == 4) || (s->kl == 4))
-                       {
-                       printf("length-L: %f, f: %f, len: %f, L: %f\n", length-L, (k*(length-L)), length, L);
-                       printf("kl X-x: %f, f-y: %f, f-z: %f\n", X[s->kl][0], X[s->kl][1], X[s->kl][2]);
-                       printf("ij X-x: %f, f-y: %f, f-z: %f\n\n", X[s->ij][0], X[s->ij][1], X[s->ij][2]);
-                       }
-                       */
+                       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)); 
                        
                        // TODO: verify, half verified (couldn't see error)
                        mul_fvector_S(stretch_force, dir, k*(length-L)); 
@@ -1197,18 +1270,49 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                        VECADD(s->f, s->f, stretch_force);
 
                        // Ascher & Boxman, p.21: Damping only during elonglation
                        VECADD(s->f, s->f, stretch_force);
 
                        // Ascher & Boxman, p.21: Damping only during elonglation
-                       /* VERIFIED */
-                       mul_fvector_S(damping_force, extent, clmd->sim_parms->Cdis * (INPR(vel,extent)/dot)); 
+                       // something wrong with it...
+                       mul_fvector_S(damping_force, dir, clmd->sim_parms->Cdis * INPR(vel,dir));
                        VECADD(s->f, s->f, damping_force);
                        
                        VECADD(s->f, s->f, damping_force);
                        
-                       // TODO: verify, half verified (couldn't see error)
-                       dfdx_spring_type1(s->dfdx, extent, length, L, dot, k);
-                       
+                       /* VERIFIED */
+                       dfdx_spring(s->dfdx, dir, length, L, k);
                        
                        /* VERIFIED */
                        
                        /* VERIFIED */
-                       dfdv_damp(s->dfdv, extent, clmd->sim_parms->Cdis, dot);
+                       dfdv_damp(s->dfdv, dir, clmd->sim_parms->Cdis);
+                       
                }
        }
                }
        }
+       else if(s->type & CLOTH_SPRING_TYPE_GOAL)
+       {
+               float tvect[3];
+               
+               s->flags |= CLOTH_SPRING_FLAG_NEEDED;
+               
+               // current_position = xold + t * (newposition - xold)
+               VECSUB(tvect, verts[s->ij].xconst, verts[s->ij].xold);
+               mul_fvector_S(tvect, tvect, time);
+               VECADD(tvect, tvect, verts[s->ij].xold);
+
+               VECSUB(extent, X[s->ij], tvect);
+               
+               dot = INPR(extent, extent);
+               length = sqrt(dot);
+               
+               k = clmd->sim_parms->goalspring;
+               
+               scaling = k + s->stiffness * ABS(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.01 * INPR(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)));
+       }
        else // calculate force of bending springs
        {
                if(length < L)
        else // calculate force of bending springs
        {
                if(length < L)
@@ -1218,10 +1322,7 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                        k = clmd->sim_parms->bending;   
                        
                        scaling = k + s->stiffness * ABS(clmd->sim_parms->max_bend-k);                  
                        k = clmd->sim_parms->bending;   
                        
                        scaling = k + s->stiffness * ABS(clmd->sim_parms->max_bend-k);                  
-                       cb = k = scaling / (20.0*(clmd->coll_parms->avg_spring_len + FLT_EPSILON));
-                       
-                       if(G.rt>3)
-                       printf("bend scaling: %f\n", k);
+                       cb = k = scaling / (20.0*(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);
 
                        mul_fvector_S(bending_force, dir, fbstar(length, L, k, cb));
                        VECADD(s->f, s->f, bending_force);
@@ -1229,19 +1330,13 @@ DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s,
                        dfdx_spring_type2(s->dfdx, dir, length,L, k, cb);
                }
        }
                        dfdx_spring_type2(s->dfdx, dir, length,L, k, cb);
                }
        }
-       /*
-       if((s->ij == 109) || (s->kl == 109))
-       {
-       printf("type: %d, f-x: %f, f-y: %f, f-z: %f\n", s->type, s->f[0], s->f[1], s->f[2]);
-}
-       */
 }
 
 DO_INLINE void cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
 {
        if(s->flags & CLOTH_SPRING_FLAG_NEEDED)
        {
 }
 
 DO_INLINE void cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
 {
        if(s->flags & CLOTH_SPRING_FLAG_NEEDED)
        {
-               if(s->type != CLOTH_SPRING_TYPE_BENDING)
+               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);
                {
                        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);
@@ -1249,70 +1344,33 @@ DO_INLINE void cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s,
                }
 
                VECADD(lF[s->ij], lF[s->ij], s->f);
                }
 
                VECADD(lF[s->ij], lF[s->ij], s->f);
-               VECSUB(lF[s->kl], lF[s->kl], s->f);
-
-               sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx);
+               
+               if(!(s->type & CLOTH_SPRING_TYPE_GOAL))
+                       VECSUB(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->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);
        }       
 }
 
                add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx);
        }       
 }
 
-DO_INLINE void calculateTriangleNormal(float to[3], lfVector *X, MFace mface)
-{
-       float v1[3], v2[3];
-
-       VECSUB(v1, X[mface.v2], X[mface.v1]);
-       VECSUB(v2, X[mface.v3], X[mface.v1]);
-       cross_fvector(to, v1, v2);
-}
-
-DO_INLINE void calculatQuadNormal(float to[3], lfVector *X, MFace mface)
-{
-       float temp = CalcNormFloat4(X[mface.v1],X[mface.v2],X[mface.v3],X[mface.v4],to);
-       mul_fvector_S(to, to, temp);
-}
-
-void calculateWeightedVertexNormal(ClothModifierData *clmd, MFace *mfaces, float to[3], int index, lfVector *X)
-{
-       float temp[3]; 
-       int i;
-       Cloth *cloth = clmd->clothObject;
-
-       for(i = 0; i < cloth->numfaces; i++)
-       {
-               // check if this triangle contains the selected vertex
-               if(mfaces[i].v1 == index || mfaces[i].v2 == index || mfaces[i].v3 == index || mfaces[i].v4 == index)
-               {
-                       calculatQuadNormal(temp, X, mfaces[i]);
-                       VECADD(to, to, temp);
-               }
-       }
-}
 float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
 {
 float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
 {
-       return fabs(INPR(wind, vertexnormal) * 0.5f);
-}
-
-DO_INLINE void calc_triangle_force(ClothModifierData *clmd, MFace mface, lfVector *F, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors)
-{      
-
+       return fabs(INPR(wind, vertexnormal));
 }
 
 void cloth_calc_force(ClothModifierData *clmd, 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;
 }
 
 void cloth_calc_force(ClothModifierData *clmd, 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;
-       unsigned int    i               = 0;
+       long            i               = 0;
        float           spring_air      = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */
        float           gravity[3];
        float           tm2[3][3]       = {{-spring_air,0,0}, {0,-spring_air,0},{0,0,-spring_air}};
        float           spring_air      = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */
        float           gravity[3];
        float           tm2[3][3]       = {{-spring_air,0,0}, {0,-spring_air,0},{0,0,-spring_air}};
-       ClothVertex *verts = cloth->verts;
        MFace           *mfaces         = cloth->mfaces;
        MFace           *mfaces         = cloth->mfaces;
+       //ClothVertex   *verts          = cloth->verts;
        float wind_normalized[3];
        unsigned int numverts = cloth->numverts;
        float wind_normalized[3];
        unsigned int numverts = cloth->numverts;
-       float auxvect[3], velgoal[3], tvect[3];
-       float kd, ks;
        LinkNode *search = cloth->springs;
 
 
        LinkNode *search = cloth->springs;
 
 
@@ -1326,8 +1384,10 @@ void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVec
 
        init_lfvector(lF, gravity, numverts);
        
 
        init_lfvector(lF, gravity, numverts);
        
-       // multiply lF with mass matrix
-       for(i = 0; i < numverts; i++)
+       /* multiply lF with mass matrix
+       // force = mass * acceleration (in this case: gravity)
+       */
+       for(i = 0; i < (long)numverts; i++)
        {
                float temp[3];
                VECCOPY(temp, lF[i]);
        {
                float temp[3];
                VECCOPY(temp, lF[i]);
@@ -1335,56 +1395,73 @@ void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVec
        }
 
        submul_lfvectorS(lF, lV, spring_air, numverts);
        }
 
        submul_lfvectorS(lF, lV, spring_air, numverts);
-               
-       /* do goal stuff */
-       if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
-       {       
-               for(i = 0; i < numverts; i++)
-               {                       
-                       if(verts [i].goal < SOFTGOALSNAP)
-                       {                       
-                               // current_position = xold + t * (newposition - xold)
-                               VECSUB(tvect, verts[i].xconst, verts[i].xold);
-                               mul_fvector_S(tvect, tvect, time);
-                               VECADD(tvect, tvect, verts[i].xold);
-
-                               VECSUB(auxvect, tvect, lX[i]);
-                               ks  = 1.0f/(1.0f- verts [i].goal*clmd->sim_parms->goalspring)-1.0f ;
-                               VECADDS(lF[i], lF[i], auxvect, -ks);
-
-                               // calulate damping forces generated by goals
-                               
-                               VECSUB(velgoal,verts[i].xold, verts[i].xconst);
-                               kd =  clmd->sim_parms->goalfrict * 0.01f; // friction force scale taken from SB
-                               VECSUBADDSS(lF[i], velgoal, kd, lV[i], kd);
-                               
-                               
-                       }
-               }       
-       }
        
        /* handle external forces like wind */
        if(effectors)
        
        /* handle external forces like wind */
        if(effectors)
-       {
-               float speed[3] = {0.0f, 0.0f,0.0f};
-               float force[3]= {0.0f, 0.0f, 0.0f};
-               
-#pragma omp parallel for private (i) shared(lF)
-               for(i = 0; i < cloth->numverts; i++)
+       {       
+               for(i = 0; i < cloth->numfaces; i++)
                {
                        float vertexnormal[3]={0,0,0};
                {
                        float vertexnormal[3]={0,0,0};
-                       float fieldfactor = 1000.0f; // windfactor  = 250.0f; // from sb
+                       float speed[3] = {0.0f, 0.0f,0.0f};
+                       float force[3]= {0.0f, 0.0f, 0.0f};
                        
                        
-                       pdDoEffectors(effectors, lX[i], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);          
+                       if(mfaces[i].v4)
+                               CalcNormFloat4(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],lX[mfaces[i].v4],vertexnormal);
+                       else
+                               CalcNormFloat(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],vertexnormal);
                        
                        
-                       // TODO apply forcefields here
-                       VECADDS(lF[i], lF[i], force, fieldfactor*0.01f);
-
+                       pdDoEffectors(effectors, lX[mfaces[i].v1], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
                        VECCOPY(wind_normalized, speed);
                        Normalize(wind_normalized);
                        VECCOPY(wind_normalized, speed);
                        Normalize(wind_normalized);
+                       VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
+                       
+                       if(mfaces[i].v4)
+                       {
+                               VECADDS(lF[mfaces[i].v1], lF[mfaces[i].v1], wind_normalized, 0.25);
+                       }
+                       else
+                       {
+                               VECADDS(lF[mfaces[i].v1], lF[mfaces[i].v1], wind_normalized, 1.0 / 3.0);
+                       }
+                       
+                       speed[0] = speed[1] = speed[2] = 0.0;
+                       pdDoEffectors(effectors, lX[mfaces[i].v2], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
+                       VECCOPY(wind_normalized, speed);
+                       Normalize(wind_normalized);
+                       VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
+                       if(mfaces[i].v4)
+                       {
+                               VECADDS(lF[mfaces[i].v2], lF[mfaces[i].v2], wind_normalized, 0.25);
+                       }
+                       else
+                       {
+                               VECADDS(lF[mfaces[i].v2], lF[mfaces[i].v2], wind_normalized, 1.0 / 3.0);
+                       }
+                       
+                       speed[0] = speed[1] = speed[2] = 0.0;
+                       pdDoEffectors(effectors, lX[mfaces[i].v3], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
+                       VECCOPY(wind_normalized, speed);
+                       Normalize(wind_normalized);
+                       VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
+                       if(mfaces[i].v4)
+                       {
+                               VECADDS(lF[mfaces[i].v3], lF[mfaces[i].v3], wind_normalized, 0.25);
+                       }
+                       else
+                       {
+                               VECADDS(lF[mfaces[i].v3], lF[mfaces[i].v3], wind_normalized, 1.0 / 3.0);
+                       }
+                       
+                       speed[0] = speed[1] = speed[2] = 0.0;
+                       if(mfaces[i].v4)
+                       {
+                               pdDoEffectors(effectors, lX[mfaces[i].v4], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
+                               VECCOPY(wind_normalized, speed);
+                               Normalize(wind_normalized);
+                               VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
+                               VECADDS(lF[mfaces[i].v4], lF[mfaces[i].v4], wind_normalized, 0.25);
+                       }
                        
                        
-                       calculateWeightedVertexNormal(clmd, mfaces, vertexnormal, i, lX);
-                       VECADDS(lF[i], lF[i], wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
                }
        }
                
                }
        }
                
@@ -1394,7 +1471,7 @@ void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVec
        {
                // 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)){}
        {
                // 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);
+               cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX, time);
 
                search = search->next;
        }
 
                search = search->next;
        }
@@ -1411,7 +1488,7 @@ void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVec
        // printf("\n");
 }
 
        // printf("\n");
 }
 
-void simulate_implicit_euler(lfVector *Vnew, lfVector *lX, lfVector *lV, lfVector *lF, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float dt, fmatrix3x3 *A, lfVector *B, lfVector *dV, fmatrix3x3 *S, lfVector *z, lfVector *olddV, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *M)
+void simulate_implicit_euler(lfVector *Vnew, lfVector *lX, lfVector *lV, lfVector *lF, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float dt, fmatrix3x3 *A, lfVector *B, lfVector *dV, fmatrix3x3 *S, lfVector *z, lfVector *olddV, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *M, fmatrix3x3 *bigI)
 {
        unsigned int numverts = dFdV[0].vcount;
 
 {
        unsigned int numverts = dFdV[0].vcount;
 
@@ -1429,7 +1506,7 @@ void simulate_implicit_euler(lfVector *Vnew, lfVector *lX, lfVector *lV, lfVecto
        itstart();
        
        cg_filtered(dV, A, B, z, S); /* conjugate gradient algorithm to solve Ax=b */
        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);
+       // cg_filtered_pre(dV, A, B, z, S, P, Pinv, bigI);
        
        itend();
        // printf("cg_filtered calc time: %f\n", (float)itval());
        
        itend();
        // printf("cg_filtered calc time: %f\n", (float)itval());
@@ -1446,11 +1523,11 @@ void simulate_implicit_euler(lfVector *Vnew, lfVector *lX, lfVector *lV, lfVecto
 int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
 {              
        unsigned int i=0;
 int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
 {              
        unsigned int i=0;
-       float step=0.0f, tf=1.0f;
+       float step=0.0f, tf=clmd->sim_parms->timescale;
        Cloth *cloth = clmd->clothObject;
        ClothVertex *verts = cloth->verts;
        unsigned int numverts = cloth->numverts;
        Cloth *cloth = clmd->clothObject;
        ClothVertex *verts = cloth->verts;
        unsigned int numverts = cloth->numverts;
-       float dt = 1.0f / clmd->sim_parms->stepsPerFrame;
+       float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
        Implicit_Data *id = cloth->implicit;
        int result = 0;
        
        Implicit_Data *id = cloth->implicit;
        int result = 0;
        
@@ -1459,35 +1536,44 @@ int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase
                for(i = 0; i < numverts; i++)
                {                       
                        // update velocities with constrained velocities from pinned verts
                for(i = 0; i < numverts; i++)
                {                       
                        // update velocities with constrained velocities from pinned verts
-                       if(verts [i].goal >= SOFTGOALSNAP)
+                       if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
                        {                       
                                VECSUB(id->V[i], verts[i].xconst, verts[i].xold);
                        {                       
                                VECSUB(id->V[i], verts[i].xconst, verts[i].xold);
-                               // VecMulf(id->V[i], 1.0 / dt);
+                               // VecMulf(id->V[i], clmd->sim_parms->stepsPerFrame);
                        }
                }       
        }
        
        while(step < tf)
                        }
                }       
        }
        
        while(step < tf)
-       {               
+       {       
+               // calculate forces
                effectors= pdInitEffectors(ob,NULL);
                effectors= pdInitEffectors(ob,NULL);
-               
-               // calculate 
                cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M);
                cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M);
+               if(effectors) pdEndEffectors(effectors);
                
                
-               // printf("F -> x: %f, y: %f; z: %f\n\n", id->F[109][0], id->F[109][1], id->F[109][2]);
-               
-               simulate_implicit_euler(id->Vnew, id->X, id->V, id->F, id->dFdV, id->dFdX, dt, id->A, id->B, id->dV, id->S, id->z, id->olddV, id->P, id->Pinv, id->M);
+               // calculate new velocity
+               simulate_implicit_euler(id->Vnew, id->X, id->V, id->F, id->dFdV, id->dFdX, dt, id->A, id->B, id->dV, id->S, id->z, id->olddV, id->P, id->Pinv, id->M, id->bigI);
                
                
+               // advance positions
                add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
                add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
-               /*
-               printf("dt: %f\n", dt);
-               printf("Xnew -> x: %f, y: %f; z: %f\n", id->Xnew[4][0], id->Xnew[4][1], id->Xnew[4][2]);
-               printf("X    -> x: %f, y: %f; z: %f\n", id->X[4][0], id->X[4][1], id->X[4][2]);
-               printf("Vnew -> x: %f, y: %f; z: %f\n", id->Vnew[4][0], id->Vnew[3][1], id->Vnew[4][2]);
-               printf("V -> x: %f, y: %f; z: %f\n\n", id->V[4][0], id->V[4][1], id->V[4][2]);
-               */
                
                
-               // clmd->coll_parms->flags &= ~CLOTH_COLLSETTINGS_FLAG_ENABLED;
+               /* 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 tvect[3] = {.0,.0,.0};
+                                       VECSUB(tvect, verts[i].xconst, verts[i].xold);
+                                       mul_fvector_S(tvect, tvect, step+dt);
+                                       VECADD(tvect, tvect, verts[i].xold);
+                                       VECCOPY(id->Xnew[i], tvect);
+                               }       
+                       }
+                       
+                       VECCOPY(verts[i].txold, id->X[i]);
+               }
                
                if(clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED)
                {
                
                if(clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED)
                {
@@ -1496,57 +1582,35 @@ int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase
                        
                        // update verts to current positions
                        for(i = 0; i < numverts; i++)
                        
                        // update verts to current positions
                        for(i = 0; i < numverts; i++)
-                       {               
-                               
-                               if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
-                               {                       
-                                       if(verts [i].goal >= SOFTGOALSNAP)
-                                       {                       
-                                               float tvect[3] = {.0,.0,.0};
-                                               // VECSUB(tvect, id->Xnew[i], verts[i].xold);
-                                               mul_fvector_S(tvect, id->V[i], step+dt);
-                                               VECADD(tvect, tvect, verts[i].xold);
-                                               VECCOPY(id->Xnew[i], tvect);
-                                       }
-                                               
-                               }
-                               
+                       {       
                                VECCOPY(verts[i].tx, id->Xnew[i]);
                                
                                VECSUB(verts[i].tv, verts[i].tx, verts[i].txold);
                                VECCOPY(verts[i].v, verts[i].tv);
                        }
                                VECCOPY(verts[i].tx, id->Xnew[i]);
                                
                                VECSUB(verts[i].tv, verts[i].tx, verts[i].txold);
                                VECCOPY(verts[i].v, verts[i].tv);
                        }
-       
+                       
                        // call collision function
                        // call collision function
-                       result = cloth_bvh_objcollision(clmd, step + dt, dt);
-       
+                       // TODO: check if "step" or "step+dt" is correct - dg
+                       result = cloth_bvh_objcollision(ob, clmd, step, dt);
+                       
+                       // correct velocity again, just to be sure we had to change it due to adaptive collisions
+                       for(i = 0; i < numverts; i++)
+                       {
+                               VECSUB(verts[i].tv, verts[i].tx, id->X[i]);
+                       }
+                       
                        // copy corrected positions back to simulation
                        for(i = 0; i < numverts; i++)
                        {               
                                if(result)
                                {
                                        
                        // copy corrected positions back to simulation
                        for(i = 0; i < numverts; i++)
                        {               
                                if(result)
                                {
                                        
-                                       if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
-                                       {                       
-                                               if(verts [i].goal >= SOFTGOALSNAP)
-                                               {
-                                                       continue;
-                                               }
-                                       }
-                                       
-                                               
-                                       // VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
-                                       
-                                       VECCOPY(verts[i].txold, verts[i].tx);
+                                       if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
+                                               continue;
                                        
                                        VECCOPY(id->Xnew[i], verts[i].tx);
                                        
                                        VECCOPY(id->Xnew[i], verts[i].tx);
-                                       
                                        VECCOPY(id->Vnew[i], verts[i].tv);
                                        VECCOPY(id->Vnew[i], verts[i].tv);
-                                       VecMulf(id->Vnew[i], 1.0f / dt);
-                               }
-                               else
-                               {
-                                       VECCOPY(verts[i].txold, id->Xnew[i]);
+                                       VecMulf(id->Vnew[i], clmd->sim_parms->stepsPerFrame);
                                }
                        }
                        
                                }
                        }
                        
@@ -1554,15 +1618,20 @@ int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase
                        cp_lfvector(id->X, id->Xnew, numverts);
                        
                        // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
                        cp_lfvector(id->X, id->Xnew, numverts);
                        
                        // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
+                       
                        if(result)
                        {
                                // V = Vnew;
                                cp_lfvector(id->V, id->Vnew, numverts);
                                
                                // calculate 
                        if(result)
                        {
                                // V = Vnew;
                                cp_lfvector(id->V, id->Vnew, numverts);
                                
                                // calculate 
-                               cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, 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);
+                               effectors= pdInitEffectors(ob,NULL);
+                               cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step+dt, id->M);     
+                               if(effectors) pdEndEffectors(effectors);
+                               
+                               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);
                        }
                        }
+                       
                }
                else
                {
                }
                else
                {
@@ -1575,35 +1644,18 @@ int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase
                
                // V = Vnew;
                cp_lfvector(id->V, id->Vnew, numverts);
                
                // V = Vnew;
                cp_lfvector(id->V, id->Vnew, numverts);
-
+               
                step += dt;
                step += dt;
-
-               if(effectors) pdEndEffectors(effectors);
                
                
-               /* ask for user break */ 
-               /*
-               don't work very well, gives problems with ctrl-a + esc
-               if (CT_localInterruptCallBack && CT_localInterruptCallBack())
-                       return 0;
-               */
        }
 
        for(i = 0; i < numverts; i++)
        {                               
        }
 
        for(i = 0; i < numverts; i++)
        {                               
-               if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL)
+               if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
                {
                {
-                       if(verts [i].goal < SOFTGOALSNAP)
-                       {
-                               VECCOPY(verts[i].txold, id->X[i]);
-                               VECCOPY(verts[i].x, id->X[i]);
-                               VECCOPY(verts[i].v, id->V[i]);
-                       }
-                       else
-                       {
-                               VECCOPY(verts[i].txold, verts[i].xconst);
-                               VECCOPY(verts[i].x, verts[i].xconst);
-                               VECCOPY(verts[i].v, id->V[i]);
-                       }
+                       VECCOPY(verts[i].txold, verts[i].xconst); // TODO: test --> should be .x 
+                       VECCOPY(verts[i].x, verts[i].xconst);
+                       VECCOPY(verts[i].v, id->V[i]);
                }
                else
                {
                }
                else
                {
@@ -1612,6 +1664,7 @@ int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase
                        VECCOPY(verts[i].v, id->V[i]);
                }
        }
                        VECCOPY(verts[i].v, id->V[i]);
                }
        }
+       
        return 1;
 }
 
        return 1;
 }
 
@@ -1631,8 +1684,3 @@ void implicit_set_positions (ClothModifierData *clmd)
                printf("implicit_set_positions\n");     
 }
 
                printf("implicit_set_positions\n");     
 }
 
-/* Cloth global visible functions */ 
-void clmdSetInterruptCallBack(int (*f)(void))
-{
-       CT_localInterruptCallBack = f;
-}