svn merge -r 14721:14810 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / source / blender / blenkernel / intern / collision.c
index f3637b4dda2e32d64379873f8a64f6ff962933f1..9ba47874d3cfbc86491cb6d6cebc9c45bd7ed068 100644 (file)
@@ -183,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
+      /* 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
+      /* 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;
@@ -232,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 ;
@@ -305,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 ;
@@ -319,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;
        }
@@ -736,21 +762,72 @@ int cloth_are_edges_adjacent ( ClothModifierData *clmd, CollisionModifierData *c
        return 0;
 }
 
-void cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
+int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
 {
        EdgeCollPair edgecollpair;
        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 = 0;
+       float mintime = FLT_MAX;
+       float distance;
+       float triA[3][3], triB[3][3];
+       int result = 0;
 
        cloth1 = clmd->clothObject;
        verts1 = cloth1->verts;
+       
+       /*
+       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++)
        {
                // 9 edge - edge possibilities
@@ -831,18 +908,28 @@ void cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDa
                if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
                {
                        // always put coll points in p21/p22
-                       VECSUB ( a, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
-                       VECSUB ( b, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
-                       VECSUB ( c, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
-                       VECSUB ( d, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
-                       VECSUB ( e, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold );
-                       VECSUB ( f, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].v );
-       
-                       numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution );
+                       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++ )
                        {
-                               if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) )
+                               if ( ( solution[k] >= DBL_EPSILON ) && ( solution[k] <= 1.0 ) )
                                {
                                        //float out_collisionTime = solution[k];
        
@@ -850,14 +937,35 @@ void cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDa
        
                                        // TODO: put into (edge) collision list
                                        
-                                       mintime = MIN2(mintime, solution[k]);
-       
-                                       printf("Moving edge found!, mintime: %f\n", mintime);
+                                       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 )
@@ -868,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++ )
        {
@@ -932,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];
 
@@ -982,6 +1091,8 @@ int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *col
                
                cloth_collision_moving_edges ( clmd, collmd, collpair);
        }
+       
+       return 1;
 }
 
 int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData *collmd, float step, float dt )