svn merge -r 14721:14810 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / source / blender / blenkernel / intern / collision.c
index e244ccca306304b80feb18842c8a7bd5603e41c2..9ba47874d3cfbc86491cb6d6cebc9c45bd7ed068 100644 (file)
@@ -41,7 +41,6 @@
 #include "BKE_global.h"
 #include "BKE_mesh.h"
 #include "BKE_object.h"
-#include "BKE_cloth.h"
 #include "BKE_modifier.h"
 #include "BKE_utildefines.h"
 #include "BKE_DerivedMesh.h"
 
 #include "Bullet-C-Api.h"
 
+#include "BLI_kdopbvh.h"
+#include "BKE_collision.h"
+
+#ifdef _WIN32
+static void start ( void )
+{}
+static void end ( void )
+{
+}
+static double val()
+{
+       return 0;
+}
+#else
+#include <sys/time.h>
+static void mystart ( struct timeval *start, struct timezone *z )
+{
+       gettimeofday ( start, z );
+}
+static void myend ( struct timeval *end, struct timezone *z )
+{
+       gettimeofday ( end,z );
+}
+static double myval ( struct timeval *start, struct timeval *end )
+{
+       double t1, t2;
+       t1 = ( double ) start->tv_sec + ( double ) start->tv_usec/ ( 1000*1000 );
+       t2 = ( double ) end->tv_sec + ( double ) end->tv_usec/ ( 1000*1000 );
+       return t2-t1;
+}
+#endif
+
 /***********************************
 Collision modifier code start
 ***********************************/
@@ -66,58 +97,80 @@ void collision_move_object ( CollisionModifierData *collmd, float step, float pr
                VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step );
                VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co );
        }
-       bvh_update_from_mvert ( collmd->bvh, collmd->current_x, collmd->numverts, collmd->current_xnew, 1 );
+       bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
 }
 
-/* build bounding volume hierarchy from mverts (see kdop.c for whole BVH code) */
-BVH *bvh_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int numverts, float epsilon )
+BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int numverts, float epsilon )
 {
-       BVH *bvh=NULL;
-
-       bvh = MEM_callocN ( sizeof ( BVH ), "BVH" );
-       if ( bvh == NULL )
-       {
-               printf ( "bvh: Out of memory.\n" );
-               return NULL;
-       }
+       BVHTree *tree;
+       float co[12];
+       int i;
+       MFace *tface = mfaces;
 
-       // in the moment, return zero if no faces there
-       if ( !numfaces )
-               return NULL;
+       tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
 
-       bvh->epsilon = epsilon;
-       bvh->numfaces = numfaces;
-       bvh->mfaces = mfaces;
-
-       // we have no faces, we save seperate points
-       if ( !mfaces )
+       // fill tree
+       for ( i = 0; i < numfaces; i++, tface++ )
        {
-               bvh->numfaces = numverts;
-       }
+               VECCOPY ( &co[0*3], x[tface->v1].co );
+               VECCOPY ( &co[1*3], x[tface->v2].co );
+               VECCOPY ( &co[2*3], x[tface->v3].co );
+               if ( tface->v4 )
+                       VECCOPY ( &co[3*3], x[tface->v4].co );
 
-       bvh->numverts = numverts;
-       bvh->current_x = MEM_dupallocN ( x );
+               BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
+       }
 
-       bvh_build ( bvh );
+       // balance tree
+       BLI_bvhtree_balance ( tree );
 
-       return bvh;
+       return tree;
 }
 
-void bvh_update_from_mvert ( BVH * bvh, MVert *x, unsigned int numverts, MVert *xnew, int moving )
+void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int numverts, int moving )
 {
-       if ( !bvh )
-               return;
+       int i;
+       MFace *mfaces = faces;
+       float co[12], co_moving[12];
+       int ret = 0;
 
-       if ( numverts!=bvh->numverts )
+       if ( !bvhtree )
                return;
 
        if ( x )
-               memcpy ( bvh->current_xold, x, sizeof ( MVert ) * numverts );
+       {
+               for ( i = 0; i < numfaces; i++, mfaces++ )
+               {
+                       VECCOPY ( &co[0*3], x[mfaces->v1].co );
+                       VECCOPY ( &co[1*3], x[mfaces->v2].co );
+                       VECCOPY ( &co[2*3], x[mfaces->v3].co );
+                       if ( mfaces->v4 )
+                               VECCOPY ( &co[3*3], x[mfaces->v4].co );
+
+                       // copy new locations into array
+                       if ( moving && xnew )
+                       {
+                               // update moving positions
+                               VECCOPY ( &co_moving[0*3], xnew[mfaces->v1].co );
+                               VECCOPY ( &co_moving[1*3], xnew[mfaces->v2].co );
+                               VECCOPY ( &co_moving[2*3], xnew[mfaces->v3].co );
+                               if ( mfaces->v4 )
+                                       VECCOPY ( &co_moving[3*3], xnew[mfaces->v4].co );
+
+                               ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
+                       }
+                       else
+                       {
+                               ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
+                       }
 
-       if ( xnew )
-               memcpy ( bvh->current_x, xnew, sizeof ( MVert ) * numverts );
+                       // check if tree is already full
+                       if ( !ret )
+                               break;
+               }
 
-       bvh_update ( bvh, moving );
+               BLI_bvhtree_update_tree ( bvhtree );
+       }
 }
 
 /***********************************
@@ -130,42 +183,43 @@ Collision modifier code end
  * copied from SOLVE_CUBIC.C --> GSL
  */
 
-/* DG: debug hint! don't forget that all functions were "fabs", "sinf", etc before */
-#define mySWAP(a,b) { float tmp = b ; b = a ; a = tmp ; }
+#define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0)
 
-int gsl_poly_solve_cubic ( float a, float b, float c, float *x0, float *x1, float *x2 )
+int 
+               gsl_poly_solve_cubic (double a, double b, double c, 
+                                                         double *x0, double *x1, double *x2)
 {
-       float q = ( a * a - 3 * b );
-       float r = ( 2 * a * a * a - 9 * a * b + 27 * c );
+       double q = (a * a - 3 * b);
+       double r = (2 * a * a * a - 9 * a * b + 27 * c);
 
-       float Q = q / 9;
-       float R = r / 54;
+       double Q = q / 9;
+       double R = r / 54;
 
-       float Q3 = Q * Q * Q;
-       float R2 = R * R;
+       double Q3 = Q * Q * Q;
+       double R2 = R * R;
 
-       float CR2 = 729 * r * r;
-       float CQ3 = 2916 * q * q * q;
+       double CR2 = 729 * r * r;
+       double CQ3 = 2916 * q * q * q;
 
-       if ( R == 0 && Q == 0 )
+       if (R == 0 && Q == 0)
        {
                *x0 = - a / 3 ;
                *x1 = - a / 3 ;
                *x2 = - a / 3 ;
                return 3 ;
        }
-       else if ( CR2 == CQ3 )
+       else if (CR2 == CQ3) 
        {
-               /* this test is actually R2 == Q3, written in a form suitable
-                 for exact computation with integers */
+      /* this test is actually R2 == Q3, written in a form suitable
+               for exact computation with integers */
 
-               /* Due to finite precision some float roots may be missed, and
-                 considered to be a pair of complex roots z = x +/- epsilon i
-                 close to the real axis. */
+      /* Due to finite precision some double roots may be missed, and
+               considered to be a pair of complex roots z = x +/- epsilon i
+               close to the real axis. */
 
-               float sqrtQ = sqrt ( Q );
+               double sqrtQ = sqrt (Q);
 
-               if ( R > 0 )
+               if (R > 0)
                {
                        *x0 = -2 * sqrtQ  - a / 3;
                        *x1 = sqrtQ - a / 3;
@@ -179,72 +233,88 @@ int gsl_poly_solve_cubic ( float a, float b, float c, float *x0, float *x1, floa
                }
                return 3 ;
        }
-       else if ( CR2 < CQ3 ) /* equivalent to R2 < Q3 */
+       else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
        {
-               float sqrtQ = sqrt ( Q );
-               float sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
-               float theta = acos ( R / sqrtQ3 );
-               float norm = -2 * sqrtQ;
-               *x0 = norm * cos ( theta / 3 ) - a / 3;
-               *x1 = norm * cos ( ( theta + 2.0 * M_PI ) / 3 ) - a / 3;
-               *x2 = norm * cos ( ( theta - 2.0 * M_PI ) / 3 ) - a / 3;
-
+               double sqrtQ = sqrt (Q);
+               double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
+               double theta = acos (R / sqrtQ3);
+               double norm = -2 * sqrtQ;
+               *x0 = norm * cos (theta / 3) - a / 3;
+               *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
+               *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;
+      
                /* Sort *x0, *x1, *x2 into increasing order */
 
-               if ( *x0 > *x1 )
-                       mySWAP ( *x0, *x1 ) ;
-
-               if ( *x1 > *x2 )
+               if (*x0 > *x1)
+                       mySWAP(*x0, *x1) ;
+      
+               if (*x1 > *x2)
                {
-                       mySWAP ( *x1, *x2 ) ;
-
-                       if ( *x0 > *x1 )
-                               mySWAP ( *x0, *x1 ) ;
+                       mySWAP(*x1, *x2) ;
+          
+                       if (*x0 > *x1)
+                               mySWAP(*x0, *x1) ;
                }
-
+      
                return 3;
        }
        else
        {
-               float sgnR = ( R >= 0 ? 1 : -1 );
-               float A = -sgnR * pow ( ABS ( R ) + sqrt ( R2 - Q3 ), 1.0/3.0 );
-               float B = Q / A ;
+               double sgnR = (R >= 0 ? 1 : -1);
+               double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0);
+               double B = Q / A ;
                *x0 = A + B - a / 3;
                return 1;
        }
 }
 
 
+
 /**
  * gsl_poly_solve_quadratic
  *
  * copied from GSL
  */
-int gsl_poly_solve_quadratic ( float a, float b, float c,  float *x0, float *x1 )
+int 
+               gsl_poly_solve_quadratic (double a, double b, double c, 
+                                                                 double *x0, double *x1)
 {
-       float disc = b * b - 4 * a * c;
+       double disc = b * b - 4 * a * c;
+
+       if (a == 0) /* Handle linear case */
+       {
+               if (b == 0)
+               {
+                       return 0;
+               }
+               else
+               {
+                       *x0 = -c / b;
+                       return 1;
+               };
+       }
 
-       if ( disc > 0 )
+       if (disc > 0)
        {
-               if ( b == 0 )
+               if (b == 0)
                {
-                       float r = ABS ( 0.5 * sqrt ( disc ) / a );
+                       double r = fabs (0.5 * sqrt (disc) / a);
                        *x0 = -r;
                        *x1 =  r;
                }
                else
                {
-                       float sgnb = ( b > 0 ? 1 : -1 );
-                       float temp = -0.5 * ( b + sgnb * sqrt ( disc ) );
-                       float r1 = temp / a ;
-                       float r2 = c / temp ;
+                       double sgnb = (b > 0 ? 1 : -1);
+                       double temp = -0.5 * (b + sgnb * sqrt (disc));
+                       double r1 = temp / a ;
+                       double r2 = c / temp ;
 
-                       if ( r1 < r2 )
+                       if (r1 < r2) 
                        {
                                *x0 = r1 ;
                                *x1 = r2 ;
-                       }
-                       else
+                       } 
+                       else 
                        {
                                *x0 = r2 ;
                                *x1 = r1 ;
@@ -252,7 +322,7 @@ int gsl_poly_solve_quadratic ( float a, float b, float c,  float *x0, float *x1
                }
                return 2;
        }
-       else if ( disc == 0 )
+       else if (disc == 0) 
        {
                *x0 = -0.5 * b / a ;
                *x1 = -0.5 * b / a ;
@@ -266,79 +336,88 @@ int gsl_poly_solve_quadratic ( float a, float b, float c,  float *x0, float *x1
 
 
 
+
 /*
  * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
  *     page 4, left column
  */
-
-int cloth_get_collision_time ( float a[3], float b[3], float c[3], float d[3], float e[3], float f[3], float solution[3] )
+int cloth_get_collision_time ( double a[3], double b[3], double c[3], double d[3], double e[3], double f[3], double solution[3] )
 {
        int num_sols = 0;
 
-       float g = -a[2] * c[1] * e[0] + a[1] * c[2] * e[0] +
-                 a[2] * c[0] * e[1] - a[0] * c[2] * e[1] -
-                 a[1] * c[0] * e[2] + a[0] * c[1] * e[2];
-
-       float h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
-                 a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
-                 a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
-                 b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
-                 a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
-                 a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
-
-       float i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
-                 b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
-                 b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
-                 b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
-                 a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
-                 b[2] * c[0] * f[1] - b[0] * c[2] * f[1] +
-                 a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
-                 b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
-                 a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
-
-       float j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
-                 b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
-                 b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
+       // x^0 - checked 
+       double g =      a[0] * c[1] * e[2] - a[0] * c[2] * e[1] +
+                               a[1] * c[2] * e[0] - a[1] * c[0] * e[2] + 
+                               a[2] * c[0] * e[1] - a[2] * c[1] * e[0];
+       
+       // x^1
+       double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
+                       a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
+                       a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
+                       b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
+                       a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
+                       a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
+
+       // x^2
+       double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
+                       b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
+                       b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
+                       b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
+                       a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
+                       b[2] * c[0] * f[1] - b[0] * c[2] * f[1] + 
+                       a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
+                       b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
+                       a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
+       
+       // x^3 - checked
+       double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
+                       b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
+                       b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
+       
+       /*
+       printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]);
+       printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]);
+       printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]);
+       
+       printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
+       printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
+       printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
+       
+       printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
+       printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
+       printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
+       
+       printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
+       */
 
        // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
-       if ( ABS ( j ) > ALMOST_ZERO )
+       if ( ABS ( j ) > DBL_EPSILON )
        {
                i /= j;
                h /= j;
                g /= j;
-
                num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
        }
-       else if ( ABS ( i ) > ALMOST_ZERO )
+       else
        {
                num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
                solution[2] = -1.0;
        }
-       else if ( ABS ( h ) > ALMOST_ZERO )
-       {
-               solution[0] = -g / h;
-               solution[1] = solution[2] = -1.0;
-               num_sols = 1;
-       }
-       else if ( ABS ( g ) > ALMOST_ZERO )
-       {
-               solution[0] = 0;
-               solution[1] = solution[2] = -1.0;
-               num_sols = 1;
-       }
+
+       // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0],  solution[1],  solution[2]);
 
        // Discard negative solutions
-       if ( ( num_sols >= 1 ) && ( solution[0] < 0 ) )
+       if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) )
        {
                --num_sols;
                solution[0] = solution[num_sols];
        }
-       if ( ( num_sols >= 2 ) && ( solution[1] < 0 ) )
+       if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) )
        {
                --num_sols;
                solution[1] = solution[num_sols];
        }
-       if ( ( num_sols == 3 ) && ( solution[2] < 0 ) )
+       if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) )
        {
                --num_sols;
        }
@@ -419,24 +498,22 @@ DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float
        VECADDMUL ( to, v3, w3 );
 }
 
-int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd )
+int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
 {
        int result = 0;
-       LinkNode *search = NULL;
-       CollPair *collpair = NULL;
        Cloth *cloth1;
        float w1, w2, w3, u1, u2, u3;
        float v1[3], v2[3], relativeVelocity[3];
        float magrelVel;
-       float epsilon2 = collmd->bvh->epsilon;
+       float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
 
        cloth1 = clmd->clothObject;
 
-       search = clmd->coll_parms->collision_list;
-
-       while ( search )
+       for ( ; collpair != collision_end; collpair++ )
        {
-               collpair = search->link;
+               // only handle static collisions here
+               if ( collpair->flag & COLLISION_IN_FUTURE )
+                       continue;
 
                // compute barycentric coordinates for both collision points
                collision_compute_barycentric ( collpair->pa,
@@ -530,8 +607,6 @@ int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifier
 
                        result = 1;
                }
-
-               search = search->next;
        }
 
 
@@ -549,51 +624,46 @@ int cloth_collision_response_moving_edges ( ClothModifierData *clmd, ClothModifi
        return 1;
 }
 
-void cloth_collision_static ( ModifierData *md1, ModifierData *md2, CollisionTree *tree1, CollisionTree *tree2 )
+//Determines collisions on overlap, collisions are writen to collpair[i] and collision+number_collision_found is returned
+CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, CollPair *collpair )
 {
        ClothModifierData *clmd = ( ClothModifierData * ) md1;
        CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
-       CollPair *collpair = NULL;
-       Cloth *cloth1=NULL;
-       MFace *face1=NULL, *face2=NULL;
-       ClothVertex *verts1=NULL;
+       MFace *face1=NULL, *face2 = NULL;
+       ClothVertex *verts1 = clmd->clothObject->verts;
        double distance = 0;
-       float epsilon = clmd->coll_parms->epsilon;
-       float epsilon2 = ( ( CollisionModifierData * ) md2 )->bvh->epsilon;
-       unsigned int i = 0;
+       float epsilon1 = clmd->coll_parms->epsilon;
+       float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
+       int i;
+
+       face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
+       face2 = & ( collmd->mfaces[overlap->indexB] );
 
+       // check all 4 possible collisions
        for ( i = 0; i < 4; i++ )
        {
-               collpair = ( CollPair * ) MEM_callocN ( sizeof ( CollPair ), "cloth coll pair" );
-
-               cloth1 = clmd->clothObject;
-
-               verts1 = cloth1->verts;
-
-               face1 = & ( cloth1->mfaces[tree1->tri_index] );
-               face2 = & ( collmd->mfaces[tree2->tri_index] );
-
-               // check all possible pairs of triangles
                if ( i == 0 )
                {
+                       // fill faceA
                        collpair->ap1 = face1->v1;
                        collpair->ap2 = face1->v2;
                        collpair->ap3 = face1->v3;
 
+                       // fill faceB
                        collpair->bp1 = face2->v1;
                        collpair->bp2 = face2->v2;
                        collpair->bp3 = face2->v3;
-
                }
-
-               if ( i == 1 )
+               else if ( i == 1 )
                {
                        if ( face1->v4 )
                        {
-                               collpair->ap1 = face1->v3;
+                               // fill faceA
+                               collpair->ap1 = face1->v1;
                                collpair->ap2 = face1->v4;
-                               collpair->ap3 = face1->v1;
+                               collpair->ap3 = face1->v3;
 
+                               // fill faceB
                                collpair->bp1 = face2->v1;
                                collpair->bp2 = face2->v2;
                                collpair->bp3 = face2->v3;
@@ -601,239 +671,301 @@ void cloth_collision_static ( ModifierData *md1, ModifierData *md2, CollisionTre
                        else
                                i++;
                }
-
                if ( i == 2 )
                {
                        if ( face2->v4 )
                        {
+                               // fill faceA
                                collpair->ap1 = face1->v1;
                                collpair->ap2 = face1->v2;
                                collpair->ap3 = face1->v3;
 
-                               collpair->bp1 = face2->v3;
+                               // fill faceB
+                               collpair->bp1 = face2->v1;
                                collpair->bp2 = face2->v4;
-                               collpair->bp3 = face2->v1;
+                               collpair->bp3 = face2->v3;
                        }
                        else
-                               i+=2;
+                               break;
                }
-
-               if ( i == 3 )
+               else if ( i == 3 )
                {
-                       if ( ( face1->v4 ) && ( face2->v4 ) )
+                       if ( face1->v4 && face2->v4 )
                        {
-                               collpair->ap1 = face1->v3;
+                               // fill faceA
+                               collpair->ap1 = face1->v1;
                                collpair->ap2 = face1->v4;
-                               collpair->ap3 = face1->v1;
+                               collpair->ap3 = face1->v3;
 
-                               collpair->bp1 = face2->v3;
+                               // fill faceB
+                               collpair->bp1 = face2->v1;
                                collpair->bp2 = face2->v4;
-                               collpair->bp3 = face2->v1;
+                               collpair->bp3 = face2->v3;
                        }
                        else
-                               i++;
+                               break;
                }
 
-               // calc SIPcode (?)
-
-               if ( i < 4 )
-               {
-                       // calc distance + normal
 #ifdef WITH_BULLET
-                       distance = plNearestPoints (
-                                      verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, collmd->current_x[collpair->bp1].co, collmd->current_x[collpair->bp2].co, collmd->current_x[collpair->bp3].co, collpair->pa,collpair->pb,collpair->vector );
+               // calc distance + normal
+               distance = plNearestPoints (
+                              verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, collmd->current_x[collpair->bp1].co, collmd->current_x[collpair->bp2].co, collmd->current_x[collpair->bp3].co, collpair->pa,collpair->pb,collpair->vector );
 #else
-                       // just be sure that we don't add anything
-                       distance = 2.0 * ( epsilon + epsilon2 + ALMOST_ZERO );
+               // just be sure that we don't add anything
+               distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO );
 #endif
-                       if ( distance <= ( epsilon + epsilon2 + ALMOST_ZERO ) )
-                       {
-                               // printf("dist: %f\n", (float)distance);
-
-                               // collpair->face1 = tree1->tri_index;
-                               // collpair->face2 = tree2->tri_index;
 
-                               VECCOPY ( collpair->normal, collpair->vector );
-                               Normalize ( collpair->normal );
-
-                               collpair->distance = distance;
-                               BLI_linklist_prepend ( &clmd->coll_parms->collision_list, collpair );
+               if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
+               {
+                       VECCOPY ( collpair->normal, collpair->vector );
+                       Normalize ( collpair->normal );
 
-                       }
-                       else
-                       {
-                               MEM_freeN ( collpair );
-                       }
+                       collpair->distance = distance;
+                       collpair->flag = 0;
                }
                else
                {
-                       MEM_freeN ( collpair );
+                       // check for collision in the future
+                       collpair->flag |= COLLISION_IN_FUTURE;
                }
+               collpair++;
        }
+       return collpair;
 }
 
-int cloth_are_edges_adjacent ( ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair )
+int cloth_are_edges_adjacent ( ClothModifierData *clmd, CollisionModifierData *collmd, EdgeCollPair *edgecollpair )
 {
-       Cloth *cloth1 = NULL, *cloth2 = NULL;
-       ClothVertex *verts1 = NULL, *verts2 = NULL;
+       Cloth *cloth1 = NULL;
+       ClothVertex *verts1 = NULL;
        float temp[3];
+       MVert *verts2 = collmd->current_x; // old x
 
        cloth1 = clmd->clothObject;
-       cloth2 = coll_clmd->clothObject;
-
        verts1 = cloth1->verts;
-       verts2 = cloth2->verts;
 
-       VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold );
+       VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].co );
        if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
                return 1;
 
-       VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold );
+       VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].co );
        if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
                return 1;
 
-       VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold );
+       VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].co );
        if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
                return 1;
 
-       VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold );
+       VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].co );
        if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
                return 1;
 
        return 0;
 }
 
-void cloth_collision_moving_edges ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
+int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
 {
        EdgeCollPair edgecollpair;
-       Cloth *cloth1=NULL, *cloth2=NULL;
-       MFace *face1=NULL, *face2=NULL;
-       ClothVertex *verts1=NULL, *verts2=NULL;
+       Cloth *cloth1=NULL;
+       ClothVertex *verts1=NULL;
        unsigned int i = 0, j = 0, k = 0;
        int numsolutions = 0;
-       float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
+       double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
+       double solution[3];
+       MVert *verts2 = collmd->current_x; // old x
+       MVert *velocity2 = collmd->current_v; // velocity
+       float mintime = FLT_MAX;
+       float distance;
+       float triA[3][3], triB[3][3];
+       int result = 0;
 
        cloth1 = clmd->clothObject;
-       cloth2 = coll_clmd->clothObject;
-
        verts1 = cloth1->verts;
-       verts2 = cloth2->verts;
-
-       face1 = & ( cloth1->mfaces[tree1->tri_index] );
-       face2 = & ( cloth2->mfaces[tree2->tri_index] );
-
-       for ( i = 0; i < 5; i++ )
+       
+       /*
+       double p[4][3] = {{0,0,0},{0,2,0},{1,1,-1},{1,1,1}};
+       double v[4][3] = {{0,0,0},{1,0,0},{-2,0,0},{-2,0,0}};
+       
+       double pp[2][3] = {{-1,-1,-1}, {2,2,2}};
+       
+       
+       VECSUB ( x1, p[1], p[0] );
+       VECSUB ( v1, v[1], v[0] );
+                       
+       VECSUB ( x2, p[2], p[0] );
+       VECSUB ( v2, v[2], v[0] );
+                       
+       VECSUB ( x3, p[3], p[0] );
+       VECSUB ( v3, v[3], v[0] );
+
+       printf("x1 x: %f, y: %f, z: %f\n", x1[0], x1[1], x1[2]);
+       printf("x2 x: %f, y: %f, z: %f\n", x2[0], x2[1], x2[2]);
+       printf("x3 x: %f, y: %f, z: %f\n", x3[0], x3[1], x3[2]);
+       
+       printf("v1 x: %f, y: %f, z: %f\n", v1[0], v1[1], v1[2]);
+       printf("v2 x: %f, y: %f, z: %f\n", v2[0], v2[1], v2[2]);
+       printf("v3 x: %f, y: %f, z: %f\n", v3[0], v3[1], v3[2]);
+
+       numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
+       
+       for ( k = 0; k < numsolutions; k++ )
+               printf("mintime: %f\n", solution[k]);
+       
+       mintime = solution[0];
+       
+       // move triangles to collision point in time
+       VECADDS(triA[0], pp[0], v[0], solution[0]);
+       VECADDS(triA[1], p[0], v[0], solution[0]);
+       VECADDS(triA[2], p[1], v[1], solution[0]);
+               
+       VECADDS(triB[0], pp[1], v[0], solution[0]);
+       VECADDS(triB[1], p[2], v[2], solution[0]);
+       VECADDS(triB[2], p[3], v[3], solution[0]);
+               
+               // check distance there
+       distance = plNearestPoints (triA[0], triA[1], triA[2], triB[0], triB[1], triB[2], collpair->pa,collpair->pb,collpair->vector );
+       
+       printf("mintime: %f, dist: %f\n", mintime, distance);
+       
+       exit(0);
+       */
+       for(i = 0; i < 9; i++)
        {
-               if ( i == 0 )
+               // 9 edge - edge possibilities
+               
+               if(i == 0) // cloth edge: 1-2; coll edge: 1-2
                {
-                       edgecollpair.p11 = face1->v1;
-                       edgecollpair.p12 = face1->v2;
+                       edgecollpair.p11 = collpair->ap1;
+                       edgecollpair.p12 = collpair->ap2;
+                       
+                       edgecollpair.p21 = collpair->bp1;
+                       edgecollpair.p22 = collpair->bp2;
                }
-               else if ( i == 1 )
+               else if(i == 1) // cloth edge: 1-2; coll edge: 2-3
                {
-                       edgecollpair.p11 = face1->v2;
-                       edgecollpair.p12 = face1->v3;
+                       edgecollpair.p11 = collpair->ap1;
+                       edgecollpair.p12 = collpair->ap2;
+                       
+                       edgecollpair.p21 = collpair->bp2;
+                       edgecollpair.p22 = collpair->bp3;
                }
-               else if ( i == 2 )
+               else if(i == 2) // cloth edge: 1-2; coll edge: 1-3
                {
-                       if ( face1->v4 )
-                       {
-                               edgecollpair.p11 = face1->v3;
-                               edgecollpair.p12 = face1->v4;
-                       }
-                       else
-                       {
-                               edgecollpair.p11 = face1->v3;
-                               edgecollpair.p12 = face1->v1;
-                               i+=5; // get out of here after this edge pair is handled
-                       }
+                       edgecollpair.p11 = collpair->ap1;
+                       edgecollpair.p12 = collpair->ap2;
+                       
+                       edgecollpair.p21 = collpair->bp1;
+                       edgecollpair.p22 = collpair->bp3;
                }
-               else if ( i == 3 )
+               else if(i == 3) // cloth edge: 2-3; coll edge: 1-2
                {
-                       if ( face1->v4 )
-                       {
-                               edgecollpair.p11 = face1->v4;
-                               edgecollpair.p12 = face1->v1;
-                       }
-                       else
-                               continue;
+                       edgecollpair.p11 = collpair->ap2;
+                       edgecollpair.p12 = collpair->ap3;
+                       
+                       edgecollpair.p21 = collpair->bp1;
+                       edgecollpair.p22 = collpair->bp2;
                }
-               else
+               else if(i == 4) // cloth edge: 2-3; coll edge: 2-3
                {
-                       edgecollpair.p11 = face1->v3;
-                       edgecollpair.p12 = face1->v1;
+                       edgecollpair.p11 = collpair->ap2;
+                       edgecollpair.p12 = collpair->ap3;
+                       
+                       edgecollpair.p21 = collpair->bp2;
+                       edgecollpair.p22 = collpair->bp3;
                }
-
-
-               for ( j = 0; j < 5; j++ )
+               else if(i == 5) // cloth edge: 2-3; coll edge: 1-3
                {
-                       if ( j == 0 )
-                       {
-                               edgecollpair.p21 = face2->v1;
-                               edgecollpair.p22 = face2->v2;
-                       }
-                       else if ( j == 1 )
-                       {
-                               edgecollpair.p21 = face2->v2;
-                               edgecollpair.p22 = face2->v3;
-                       }
-                       else if ( j == 2 )
-                       {
-                               if ( face2->v4 )
-                               {
-                                       edgecollpair.p21 = face2->v3;
-                                       edgecollpair.p22 = face2->v4;
-                               }
-                               else
-                               {
-                                       edgecollpair.p21 = face2->v3;
-                                       edgecollpair.p22 = face2->v1;
-                               }
-                       }
-                       else if ( j == 3 )
-                       {
-                               if ( face2->v4 )
-                               {
-                                       edgecollpair.p21 = face2->v4;
-                                       edgecollpair.p22 = face2->v1;
-                               }
-                               else
-                                       continue;
-                       }
-                       else
-                       {
-                               edgecollpair.p21 = face2->v3;
-                               edgecollpair.p22 = face2->v1;
-                       }
-
-
-                       if ( !cloth_are_edges_adjacent ( clmd, coll_clmd, &edgecollpair ) )
+                       edgecollpair.p11 = collpair->ap2;
+                       edgecollpair.p12 = collpair->ap3;
+                       
+                       edgecollpair.p21 = collpair->bp1;
+                       edgecollpair.p22 = collpair->bp3;
+               }
+               else if(i ==6) // cloth edge: 1-3; coll edge: 1-2
+               {
+                       edgecollpair.p11 = collpair->ap1;
+                       edgecollpair.p12 = collpair->ap3;
+                       
+                       edgecollpair.p21 = collpair->bp1;
+                       edgecollpair.p22 = collpair->bp2;
+               }
+               else if(i ==7) // cloth edge: 1-3; coll edge: 2-3
+               {
+                       edgecollpair.p11 = collpair->ap1;
+                       edgecollpair.p12 = collpair->ap3;
+                       
+                       edgecollpair.p21 = collpair->bp2;
+                       edgecollpair.p22 = collpair->bp3;
+               }
+               else if(i == 8) // cloth edge: 1-3; coll edge: 1-3
+               {
+                       edgecollpair.p11 = collpair->ap1;
+                       edgecollpair.p12 = collpair->ap3;
+                       
+                       edgecollpair.p21 = collpair->bp1;
+                       edgecollpair.p22 = collpair->bp3;
+               }
+               
+               if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
+               {
+                       // always put coll points in p21/p22
+                       VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
+                       VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
+                       
+                       VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
+                       VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
+                       
+                       VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold );
+                       VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv );
+                       /*
+                       printf("A x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
+                       printf("B x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
+                       printf("C x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
+                       printf("D x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
+                       printf("E x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
+                       printf("F x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
+                       exit(0);
+                       */
+                       numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
+       
+                       for ( k = 0; k < numsolutions; k++ )
                        {
-                               VECSUB ( a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold );
-                               VECSUB ( b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v );
-                               VECSUB ( c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold );
-                               VECSUB ( d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v );
-                               VECSUB ( e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold );
-                               VECSUB ( f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v );
-
-                               numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution );
-
-                               for ( k = 0; k < numsolutions; k++ )
+                               if ( ( solution[k] >= DBL_EPSILON ) && ( solution[k] <= 1.0 ) )
                                {
-                                       if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) )
-                                       {
-                                               //float out_collisionTime = solution[k];
-
-                                               // TODO: check for collisions
-
-                                               // TODO: put into (edge) collision list
-
-                                               // printf("Moving edge found!\n");
-                                       }
+                                       //float out_collisionTime = solution[k];
+       
+                                       // TODO: check for collisions
+       
+                                       // TODO: put into (edge) collision list
+                                       
+                                       mintime = MIN2(mintime, (float)solution[k]);
+                                       
+//                                     printf("mt: %f, %lf, %f\n", mintime, solution[k], (float)solution[k]);
+                                       
+                                       result = 1;
+                                       break;
                                }
                        }
                }
        }
+       
+       if(result)
+       {
+               // move triangles to collision point in time
+               VECADDS(triA[0], verts1[collpair->ap1].txold, verts1[collpair->ap1].tv, mintime);
+               VECADDS(triA[1], verts1[collpair->ap2].txold, verts1[collpair->ap2].tv, mintime);
+               VECADDS(triA[2], verts1[collpair->ap3].txold, verts1[collpair->ap3].tv, mintime);
+               
+               VECADDS(triB[0], collmd->current_x[collpair->bp1].co, collmd->current_v[collpair->bp1].co, mintime);
+               VECADDS(triB[1], collmd->current_x[collpair->bp2].co, collmd->current_v[collpair->bp2].co, mintime);
+               VECADDS(triB[2], collmd->current_x[collpair->bp3].co, collmd->current_v[collpair->bp3].co, mintime);
+               
+               // check distance there
+               distance = plNearestPoints (triA[0], triA[1], triA[2], triB[0], triB[1], triB[2], collpair->pa,collpair->pb,collpair->vector );
+               
+               printf("mintime: %f, dist: %f\n", mintime, distance);
+       }
+       
+       return result;
 }
 
 void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
@@ -844,7 +976,8 @@ void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *c
        ClothVertex *verts1=NULL, *verts2=NULL;
        unsigned int i = 0, j = 0, k = 0;
        int numsolutions = 0;
-       float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
+       float a[3], b[3], c[3], d[3], e[3], f[3];
+       double solution[3];
 
        for ( i = 0; i < 2; i++ )
        {
@@ -908,7 +1041,7 @@ void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *c
 
                                for ( k = 0; k < numsolutions; k++ )
                                {
-                                       if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) )
+                                       if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) )
                                        {
                                                //float out_collisionTime = solution[k];
 
@@ -928,6 +1061,7 @@ void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *c
        }
 }
 
+/*
 void cloth_collision_moving ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
 {
        // TODO: check for adjacent
@@ -936,51 +1070,70 @@ void cloth_collision_moving ( ClothModifierData *clmd, ClothModifierData *coll_c
        cloth_collision_moving_tris ( clmd, coll_clmd, tree1, tree2 );
        cloth_collision_moving_tris ( coll_clmd, clmd, tree2, tree1 );
 }
+*/
 
-void cloth_free_collision_list ( ClothModifierData *clmd )
+int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
 {
-       // free collision list
-       if ( clmd->coll_parms->collision_list )
-       {
-               LinkNode *search = clmd->coll_parms->collision_list;
-               while ( search )
-               {
-                       CollPair *coll_pair = search->link;
+       int result = 0;
+       Cloth *cloth1;
+       float w1, w2, w3, u1, u2, u3;
+       float v1[3], v2[3], relativeVelocity[3];
+       float magrelVel;
+       float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
 
-                       MEM_freeN ( coll_pair );
-                       search = search->next;
-               }
-               BLI_linklist_free ( clmd->coll_parms->collision_list,NULL );
+       cloth1 = clmd->clothObject;
 
-               clmd->coll_parms->collision_list = NULL;
+       for ( ; collpair != collision_end; collpair++ )
+       {
+               // only handle moving collisions here
+               if (!( collpair->flag & COLLISION_IN_FUTURE ))
+                       continue;
+               
+               cloth_collision_moving_edges ( clmd, collmd, collpair);
        }
+       
+       return 1;
 }
 
 int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData *collmd, float step, float dt )
 {
        Cloth *cloth = clmd->clothObject;
-       BVH *cloth_bvh= ( BVH * ) cloth->tree;
+       BVHTree *cloth_bvh= ( BVHTree * ) cloth->bvhtree;
        long i=0, j = 0, numfaces = 0, numverts = 0;
        ClothVertex *verts = NULL;
+       CollPair *collisions = NULL, *collisions_index = NULL;
        int ret = 0;
-       unsigned int result = 0;
+       int result = 0;
        float tnull[3] = {0,0,0};
+       BVHTreeOverlap *overlap = NULL;
+
 
        numfaces = clmd->clothObject->numfaces;
        numverts = clmd->clothObject->numverts;
 
        verts = cloth->verts;
 
-       if ( collmd->bvh )
+       if ( collmd->bvhtree )
        {
                /* get pointer to bounding volume hierarchy */
-               BVH *coll_bvh = collmd->bvh;
+               BVHTree *coll_bvh = collmd->bvhtree;
 
                /* move object to position (step) in time */
                collision_move_object ( collmd, step + dt, step );
 
                /* search for overlapping collision pairs */
-               bvh_traverse ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_static, 0 );
+               overlap = BLI_bvhtree_overlap ( cloth_bvh, coll_bvh, &result );
+
+               collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * result*4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
+               collisions_index = collisions;
+
+               for ( i = 0; i < result; i++ )
+               {
+                       collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, collisions_index );
+               }
+
+               if ( overlap )
+                       MEM_freeN ( overlap );
        }
        else
        {
@@ -994,11 +1147,15 @@ int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData
        {
                result = 0;
 
-               if ( collmd->bvh )
-                       result += cloth_collision_response_static ( clmd, collmd );
+               if ( collmd->bvhtree )
+               {
+                       result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
+                       result += cloth_collision_moving ( clmd, collmd, collisions, collisions_index );
+               }
 
                // apply impulses in parallel
                if ( result )
+               {
                        for ( i = 0; i < numverts; i++ )
                        {
                                // calculate "velocities" (just xnew = xold + v; no dt in v)
@@ -1011,12 +1168,10 @@ int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData
                                        ret++;
                                }
                        }
-
-               if ( !result )
-                       break;
+               }
        }
 
-       cloth_free_collision_list ( clmd );
+       if ( collisions ) MEM_freeN ( collisions );
 
        return ret;
 }
@@ -1028,7 +1183,7 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
        CollisionModifierData *collmd=NULL;
        Cloth *cloth=NULL;
        Object *coll_ob=NULL;
-       BVH *cloth_bvh=NULL;
+       BVHTree *cloth_bvh=NULL;
        long i=0, j = 0, numfaces = 0, numverts = 0;
        unsigned int result = 0, rounds = 0; // result counts applied collisions; ic is for debug output;
        ClothVertex *verts = NULL;
@@ -1036,14 +1191,14 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
        ClothModifierData *tclmd;
        int collisions = 0, count = 0;
 
-       if ( ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ ) || ! ( ( ( Cloth * ) clmd->clothObject )->tree ) )
+       if ( ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ ) || ! ( ( ( Cloth * ) clmd->clothObject )->bvhtree ) )
        {
                return 0;
        }
 
        cloth = clmd->clothObject;
        verts = cloth->verts;
-       cloth_bvh = ( BVH * ) cloth->tree;
+       cloth_bvh = ( BVHTree * ) cloth->bvhtree;
        numfaces = clmd->clothObject->numfaces;
        numverts = clmd->clothObject->numverts;
 
@@ -1052,12 +1207,11 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
        ////////////////////////////////////////////////////////////
 
        // update cloth bvh
-       bvh_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
+       bvhtree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
 
        do
        {
                result = 0;
-               clmd->coll_parms->collision_list = NULL;
 
                // check all collision objects
                for ( base = G.scene->base.first; base; base = base->next )
@@ -1126,80 +1280,87 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
                ////////////////////////////////////////////////////////////
                if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
                {
+
+                       MFace *mface = clmd->clothObject->mfaces;
+
                        collisions = 1;
                        verts = cloth->verts; // needed for openMP
 
+
+
+                       /*
                        for ( count = 0; count < clmd->coll_parms->self_loop_count; count++ )
                        {
-                               if ( collisions )
-                               {
-                                       collisions = 0;
-#pragma omp parallel for private(i,j, collisions) shared(verts, ret)
-                                       for ( i = 0; i < cloth->numverts; i++ )
-                                       {
-                                               for ( j = i + 1; j < cloth->numverts; j++ )
-                                               {
-                                                       float temp[3];
-                                                       float length = 0;
-                                                       float mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
+                       if ( collisions )
+                       {
+                       collisions = 0;
+                       #pragma omp parallel for private(i,j, collisions) shared(verts, ret)
+                       for ( i = 0; i < cloth->numverts; i++ )
+                       {
+                       for ( j = i + 1; j < cloth->numverts; j++ )
+                       {
+                       float temp[3];
+                       float length = 0;
+                       float mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
 
-                                                       if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
-                                                       {
-                                                               if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
-                                                                       && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
-                                                               {
-                                                                       continue;
-                                                               }
-                                                       }
+                       if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
+                       {
+                       if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
+                       && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
+                       {
+                       continue;
+                       }
+                       }
 
-                                                       VECSUB ( temp, verts[i].tx, verts[j].tx );
+                       VECSUB ( temp, verts[i].tx, verts[j].tx );
 
-                                                       if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
+                       if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
 
                                                        // check for adjacent points (i must be smaller j)
-                                                       if ( BLI_edgehash_haskey ( cloth->edgehash, i, j ) )
-                                                       {
-                                                               continue;
-                                                       }
-
-                                                       length = Normalize ( temp );
-
-                                                       if ( length < mindistance )
-                                                       {
-                                                               float correction = mindistance - length;
-
-                                                               if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
-                                                               {
-                                                                       VecMulf ( temp, -correction );
-                                                                       VECADD ( verts[j].tx, verts[j].tx, temp );
-                                                               }
-                                                               else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
-                                                               {
-                                                                       VecMulf ( temp, correction );
-                                                                       VECADD ( verts[i].tx, verts[i].tx, temp );
-                                                               }
-                                                               else
-                                                               {
-                                                                       VecMulf ( temp, -correction*0.5 );
-                                                                       VECADD ( verts[j].tx, verts[j].tx, temp );
-
-                                                                       VECSUB ( verts[i].tx, verts[i].tx, temp );
-                                                               }
-
-                                                               collisions = 1;
-
-                                                               if ( !ret )
-                                                               {
-#pragma omp critical
-                                                                       {
-                                                                               ret = 1;
-                                                                       }
-                                                               }
-                                                       }
-                                               }
-                                       }
-                               }
+                       if ( BLI_edgehash_haskey ( cloth->edgehash, i, j ) )
+                       {
+                       continue;
+                       }
+
+                       length = Normalize ( temp );
+
+                       if ( length < mindistance )
+                       {
+                       float correction = mindistance - length;
+
+                       if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
+                       {
+                       VecMulf ( temp, -correction );
+                       VECADD ( verts[j].tx, verts[j].tx, temp );
+                       }
+                       else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
+                       {
+                       VecMulf ( temp, correction );
+                       VECADD ( verts[i].tx, verts[i].tx, temp );
+                       }
+                       else
+                       {
+                       VecMulf ( temp, -correction*0.5 );
+                       VECADD ( verts[j].tx, verts[j].tx, temp );
+
+                       VECSUB ( verts[i].tx, verts[i].tx, temp );
+                       }
+
+                       collisions = 1;
+
+                       if ( !ret )
+                       {
+                       #pragma omp critical
+                       {
+                       ret = 1;
+                       }
+                       }
+                       }
+                       }
+                       }
+                       }
                        }
+                       */
                        ////////////////////////////////////////////////////////////
 
                        ////////////////////////////////////////////////////////////