Blender V2.61 - r43446

collision.c

Go to the documentation of this file.
00001 /*
00002  * ***** BEGIN GPL LICENSE BLOCK *****
00003  *
00004  * This program is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU General Public License
00006  * as published by the Free Software Foundation; either version 2
00007  * of the License, or (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program; if not, write to the Free Software Foundation,
00016  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
00017  *
00018  * The Original Code is Copyright (C) Blender Foundation
00019  * All rights reserved.
00020  *
00021  * The Original Code is: all of this file.
00022  *
00023  * Contributor(s): none yet.
00024  *
00025  * ***** END GPL LICENSE BLOCK *****
00026  */
00027 
00033 #include "MEM_guardedalloc.h"
00034 
00035 #include "BKE_cloth.h"
00036 
00037 #include "DNA_cloth_types.h"
00038 #include "DNA_group_types.h"
00039 #include "DNA_mesh_types.h"
00040 #include "DNA_object_types.h"
00041 #include "DNA_object_force.h"
00042 #include "DNA_scene_types.h"
00043 #include "DNA_meshdata_types.h"
00044 
00045 #include "BLI_blenlib.h"
00046 #include "BLI_math.h"
00047 #include "BLI_edgehash.h"
00048 #include "BLI_utildefines.h"
00049 #include "BLI_ghash.h"
00050 #include "BLI_memarena.h"
00051 #include "BLI_rand.h"
00052 
00053 #include "BKE_DerivedMesh.h"
00054 #include "BKE_global.h"
00055 #include "BKE_scene.h"
00056 #include "BKE_mesh.h"
00057 #include "BKE_object.h"
00058 #include "BKE_modifier.h"
00059 
00060 #include "BKE_DerivedMesh.h"
00061 #ifdef USE_BULLET
00062 #include "Bullet-C-Api.h"
00063 #endif
00064 #include "BLI_kdopbvh.h"
00065 #include "BKE_collision.h"
00066 
00067 #ifdef WITH_ELTOPO
00068 #include "eltopo-capi.h"
00069 #endif
00070 
00071 
00072 /***********************************
00073 Collision modifier code start
00074 ***********************************/
00075 
00076 /* step is limited from 0 (frame start position) to 1 (frame end position) */
00077 void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep )
00078 {
00079     float tv[3] = {0, 0, 0};
00080     unsigned int i = 0;
00081 
00082     for ( i = 0; i < collmd->numverts; i++ )
00083     {
00084         VECSUB ( tv, collmd->xnew[i].co, collmd->x[i].co );
00085         VECADDS ( collmd->current_x[i].co, collmd->x[i].co, tv, prevstep );
00086         VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step );
00087         VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co );
00088     }
00089 
00090     bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
00091 }
00092 
00093 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
00094 {
00095     BVHTree *tree;
00096     float co[12];
00097     unsigned int i;
00098     MFace *tface = mfaces;
00099 
00100     tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
00101 
00102     // fill tree
00103     for ( i = 0; i < numfaces; i++, tface++ )
00104     {
00105         copy_v3_v3 ( &co[0*3], x[tface->v1].co );
00106         copy_v3_v3 ( &co[1*3], x[tface->v2].co );
00107         copy_v3_v3 ( &co[2*3], x[tface->v3].co );
00108         if ( tface->v4 )
00109             copy_v3_v3 ( &co[3*3], x[tface->v4].co );
00110 
00111         BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
00112     }
00113 
00114     // balance tree
00115     BLI_bvhtree_balance ( tree );
00116 
00117     return tree;
00118 }
00119 
00120 void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
00121 {
00122     int i;
00123     MFace *mfaces = faces;
00124     float co[12], co_moving[12];
00125     int ret = 0;
00126 
00127     if ( !bvhtree )
00128         return;
00129 
00130     if ( x )
00131     {
00132         for ( i = 0; i < numfaces; i++, mfaces++ )
00133         {
00134             copy_v3_v3 ( &co[0*3], x[mfaces->v1].co );
00135             copy_v3_v3 ( &co[1*3], x[mfaces->v2].co );
00136             copy_v3_v3 ( &co[2*3], x[mfaces->v3].co );
00137             if ( mfaces->v4 )
00138                 copy_v3_v3 ( &co[3*3], x[mfaces->v4].co );
00139 
00140             // copy new locations into array
00141             if ( moving && xnew )
00142             {
00143                 // update moving positions
00144                 copy_v3_v3 ( &co_moving[0*3], xnew[mfaces->v1].co );
00145                 copy_v3_v3 ( &co_moving[1*3], xnew[mfaces->v2].co );
00146                 copy_v3_v3 ( &co_moving[2*3], xnew[mfaces->v3].co );
00147                 if ( mfaces->v4 )
00148                     copy_v3_v3 ( &co_moving[3*3], xnew[mfaces->v4].co );
00149 
00150                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
00151             }
00152             else
00153             {
00154                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
00155             }
00156 
00157             // check if tree is already full
00158             if ( !ret )
00159                 break;
00160         }
00161 
00162         BLI_bvhtree_update_tree ( bvhtree );
00163     }
00164 }
00165 
00166 /***********************************
00167 Collision modifier code end
00168 ***********************************/
00169 
00176 #define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0)
00177 #if 0 /* UNUSED */
00178 static int 
00179 gsl_poly_solve_cubic (double a, double b, double c, 
00180                       double *x0, double *x1, double *x2)
00181 {
00182     double q = (a * a - 3 * b);
00183     double r = (2 * a * a * a - 9 * a * b + 27 * c);
00184 
00185     double Q = q / 9;
00186     double R = r / 54;
00187 
00188     double Q3 = Q * Q * Q;
00189     double R2 = R * R;
00190 
00191     double CR2 = 729 * r * r;
00192     double CQ3 = 2916 * q * q * q;
00193 
00194     if (R == 0 && Q == 0)
00195     {
00196         *x0 = - a / 3 ;
00197         *x1 = - a / 3 ;
00198         *x2 = - a / 3 ;
00199         return 3 ;
00200     }
00201     else if (CR2 == CQ3) 
00202     {
00203         /* this test is actually R2 == Q3, written in a form suitable
00204         for exact computation with integers */
00205 
00206         /* Due to finite precision some double roots may be missed, and
00207         considered to be a pair of complex roots z = x +/- epsilon i
00208         close to the real axis. */
00209 
00210         double sqrtQ = sqrt (Q);
00211 
00212         if (R > 0)
00213         {
00214             *x0 = -2 * sqrtQ  - a / 3;
00215             *x1 = sqrtQ - a / 3;
00216             *x2 = sqrtQ - a / 3;
00217         }
00218         else
00219         {
00220             *x0 = - sqrtQ  - a / 3;
00221             *x1 = - sqrtQ - a / 3;
00222             *x2 = 2 * sqrtQ - a / 3;
00223         }
00224         return 3 ;
00225     }
00226     else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
00227     {
00228         double sqrtQ = sqrt (Q);
00229         double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
00230         double theta = acos (R / sqrtQ3);
00231         double norm = -2 * sqrtQ;
00232         *x0 = norm * cos (theta / 3) - a / 3;
00233         *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
00234         *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;
00235 
00236         /* Sort *x0, *x1, *x2 into increasing order */
00237 
00238         if (*x0 > *x1)
00239             mySWAP(*x0, *x1) ;
00240 
00241         if (*x1 > *x2)
00242         {
00243             mySWAP(*x1, *x2) ;
00244 
00245             if (*x0 > *x1)
00246                 mySWAP(*x0, *x1) ;
00247         }
00248 
00249         return 3;
00250     }
00251     else
00252     {
00253         double sgnR = (R >= 0 ? 1 : -1);
00254         double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0);
00255         double B = Q / A ;
00256         *x0 = A + B - a / 3;
00257         return 1;
00258     }
00259 }
00260 
00261 
00262 
00268 static int 
00269 gsl_poly_solve_quadratic (double a, double b, double c, 
00270                           double *x0, double *x1)
00271 {
00272     double disc = b * b - 4 * a * c;
00273 
00274     if (a == 0) /* Handle linear case */
00275     {
00276         if (b == 0)
00277         {
00278             return 0;
00279         }
00280         else
00281         {
00282             *x0 = -c / b;
00283             return 1;
00284         };
00285     }
00286 
00287     if (disc > 0)
00288     {
00289         if (b == 0)
00290         {
00291             double r = fabs (0.5 * sqrt (disc) / a);
00292             *x0 = -r;
00293             *x1 =  r;
00294         }
00295         else
00296         {
00297             double sgnb = (b > 0 ? 1 : -1);
00298             double temp = -0.5 * (b + sgnb * sqrt (disc));
00299             double r1 = temp / a ;
00300             double r2 = c / temp ;
00301 
00302             if (r1 < r2) 
00303             {
00304                 *x0 = r1 ;
00305                 *x1 = r2 ;
00306             } 
00307             else 
00308             {
00309                 *x0 = r2 ;
00310                 *x1 = r1 ;
00311             }
00312         }
00313         return 2;
00314     }
00315     else if (disc == 0) 
00316     {
00317         *x0 = -0.5 * b / a ;
00318         *x1 = -0.5 * b / a ;
00319         return 2 ;
00320     }
00321     else
00322     {
00323         return 0;
00324     }
00325 }
00326 #endif /* UNUSED */
00327 
00328 
00329 
00330 /*
00331 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
00332 *     page 4, left column
00333 */
00334 #if 0
00335 static 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] )
00336 {
00337     int num_sols = 0;
00338 
00339     // x^0 - checked 
00340     double g =  a[0] * c[1] * e[2] - a[0] * c[2] * e[1] +
00341         a[1] * c[2] * e[0] - a[1] * c[0] * e[2] + 
00342         a[2] * c[0] * e[1] - a[2] * c[1] * e[0];
00343 
00344     // x^1
00345     double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
00346         a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
00347         a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
00348         b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
00349         a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
00350         a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
00351 
00352     // x^2
00353     double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
00354         b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
00355         b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
00356         b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
00357         a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
00358         b[2] * c[0] * f[1] - b[0] * c[2] * f[1] + 
00359         a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
00360         b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
00361         a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
00362 
00363     // x^3 - checked
00364     double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
00365         b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
00366         b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
00367 
00368     /*
00369     printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]);
00370     printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]);
00371     printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]);
00372 
00373     printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
00374     printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
00375     printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
00376 
00377     printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
00378     printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
00379     printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
00380 
00381     printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
00382     
00383 */
00384     // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
00385     if ( ABS ( j ) > DBL_EPSILON )
00386     {
00387         i /= j;
00388         h /= j;
00389         g /= j;
00390         num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
00391     }
00392     else
00393     {
00394         num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
00395         solution[2] = -1.0;
00396     }
00397 
00398     // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0],  solution[1],  solution[2]);
00399 
00400     // Discard negative solutions
00401     if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) )
00402     {
00403         --num_sols;
00404         solution[0] = solution[num_sols];
00405     }
00406     if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) )
00407     {
00408         --num_sols;
00409         solution[1] = solution[num_sols];
00410     }
00411     if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) )
00412     {
00413         --num_sols;
00414     }
00415 
00416     // Sort
00417     if ( num_sols == 2 )
00418     {
00419         if ( solution[0] > solution[1] )
00420         {
00421             double tmp = solution[0];
00422             solution[0] = solution[1];
00423             solution[1] = tmp;
00424         }
00425     }
00426     else if ( num_sols == 3 )
00427     {
00428 
00429         // Bubblesort
00430         if ( solution[0] > solution[1] )
00431         {
00432             double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
00433         }
00434         if ( solution[1] > solution[2] )
00435         {
00436             double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
00437         }
00438         if ( solution[0] > solution[1] )
00439         {
00440             double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
00441         }
00442     }
00443 
00444     return num_sols;
00445 }
00446 #endif
00447 
00448 
00449 // w3 is not perfect
00450 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
00451 {
00452     double  tempV1[3], tempV2[3], tempV4[3];
00453     double  a,b,c,d,e,f;
00454 
00455     VECSUB ( tempV1, p1, p3 );
00456     VECSUB ( tempV2, p2, p3 );
00457     VECSUB ( tempV4, pv, p3 );
00458 
00459     a = INPR ( tempV1, tempV1 );
00460     b = INPR ( tempV1, tempV2 );
00461     c = INPR ( tempV2, tempV2 );
00462     e = INPR ( tempV1, tempV4 );
00463     f = INPR ( tempV2, tempV4 );
00464 
00465     d = ( a * c - b * b );
00466 
00467     if ( ABS ( d ) < (double)ALMOST_ZERO )
00468     {
00469         *w1 = *w2 = *w3 = 1.0 / 3.0;
00470         return;
00471     }
00472 
00473     w1[0] = ( float ) ( ( e * c - b * f ) / d );
00474 
00475     if ( w1[0] < 0 )
00476         w1[0] = 0;
00477 
00478     w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
00479 
00480     if ( w2[0] < 0 )
00481         w2[0] = 0;
00482 
00483     w3[0] = 1.0f - w1[0] - w2[0];
00484 }
00485 
00486 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
00487 {
00488     to[0] = to[1] = to[2] = 0;
00489     VECADDMUL ( to, v1, w1 );
00490     VECADDMUL ( to, v2, w2 );
00491     VECADDMUL ( to, v3, w3 );
00492 }
00493 
00494 #ifndef WITH_ELTOPO
00495 static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
00496 {
00497     int result = 0;
00498     Cloth *cloth1;
00499     float w1, w2, w3, u1, u2, u3;
00500     float v1[3], v2[3], relativeVelocity[3];
00501     float magrelVel;
00502     float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
00503 
00504     cloth1 = clmd->clothObject;
00505 
00506     for ( ; collpair != collision_end; collpair++ )
00507     {
00508         // only handle static collisions here
00509         if ( collpair->flag & COLLISION_IN_FUTURE )
00510             continue;
00511 
00512         // compute barycentric coordinates for both collision points
00513         collision_compute_barycentric ( collpair->pa,
00514             cloth1->verts[collpair->ap1].txold,
00515             cloth1->verts[collpair->ap2].txold,
00516             cloth1->verts[collpair->ap3].txold,
00517             &w1, &w2, &w3 );
00518 
00519         // was: txold
00520         collision_compute_barycentric ( collpair->pb,
00521             collmd->current_x[collpair->bp1].co,
00522             collmd->current_x[collpair->bp2].co,
00523             collmd->current_x[collpair->bp3].co,
00524             &u1, &u2, &u3 );
00525 
00526         // Calculate relative "velocity".
00527         collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
00528 
00529         collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
00530 
00531         VECSUB ( relativeVelocity, v2, v1 );
00532 
00533         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
00534         magrelVel = INPR ( relativeVelocity, collpair->normal );
00535 
00536         // printf("magrelVel: %f\n", magrelVel);
00537 
00538         // Calculate masses of points.
00539         // TODO
00540 
00541         // If v_n_mag < 0 the edges are approaching each other.
00542         if ( magrelVel > ALMOST_ZERO )
00543         {
00544             // Calculate Impulse magnitude to stop all motion in normal direction.
00545             float magtangent = 0, repulse = 0, d = 0;
00546             double impulse = 0.0;
00547             float vrel_t_pre[3];
00548             float temp[3], spf;
00549 
00550             // calculate tangential velocity
00551             copy_v3_v3 ( temp, collpair->normal );
00552             mul_v3_fl( temp, magrelVel );
00553             VECSUB ( vrel_t_pre, relativeVelocity, temp );
00554 
00555             // Decrease in magnitude of relative tangential velocity due to coulomb friction
00556             // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
00557             magtangent = MIN2 ( clmd->coll_parms->friction * 0.01f * magrelVel, sqrtf( INPR ( vrel_t_pre,vrel_t_pre ) ) );
00558 
00559             // Apply friction impulse.
00560             if ( magtangent > ALMOST_ZERO )
00561             {
00562                 normalize_v3( vrel_t_pre );
00563 
00564                 impulse = magtangent / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); // 2.0 *
00565                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
00566                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
00567                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
00568             }
00569 
00570             // Apply velocity stopping impulse
00571             // I_c = m * v_N / 2.0
00572             // no 2.0 * magrelVel normally, but looks nicer DG
00573             impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
00574 
00575             VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
00576             cloth1->verts[collpair->ap1].impulse_count++;
00577 
00578             VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
00579             cloth1->verts[collpair->ap2].impulse_count++;
00580 
00581             VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
00582             cloth1->verts[collpair->ap3].impulse_count++;
00583 
00584             // Apply repulse impulse if distance too short
00585             // I_r = -min(dt*kd, m(0,1d/dt - v_n))
00586             spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
00587 
00588             d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
00589             if ( ( magrelVel < 0.1f*d*spf ) && ( d > ALMOST_ZERO ) )
00590             {
00591                 repulse = MIN2 ( d*1.0f/spf, 0.1f*d*spf - magrelVel );
00592 
00593                 // stay on the safe side and clamp repulse
00594                 if ( impulse > ALMOST_ZERO )
00595                     repulse = MIN2 ( repulse, 5.0*impulse );
00596                 repulse = MAX2 ( impulse, repulse );
00597 
00598                 impulse = repulse / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
00599                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
00600                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
00601                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
00602             }
00603 
00604             result = 1;
00605         }
00606     }
00607     return result;
00608 }
00609 #endif /* !WITH_ELTOPO */
00610 
00611 #ifdef WITH_ELTOPO
00612 typedef struct edgepairkey {
00613     int a1, a2, b1, b2;
00614 } edgepairkey;
00615 
00616 unsigned int edgepair_hash(void *vkey)
00617 {
00618     edgepairkey *key = vkey;
00619     int keys[4] = {key->a1, key->a2, key->b1, key->b2};
00620     int i, j;
00621     
00622     for (i=0; i<4; i++) {
00623         for (j=0; j<3; j++) {
00624             if (keys[j] >= keys[j+1]) {
00625                 SWAP(int, keys[j], keys[j+1]);
00626             }
00627         }
00628     }
00629     
00630     return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34;
00631 }
00632 
00633 int edgepair_cmp(const void *va, const void *vb)
00634 {
00635     edgepairkey *a = va, *b = vb;
00636     int keysa[4] = {a->a1, a->a2, a->b1, a->b2};
00637     int keysb[4] = {b->a1, b->a2, b->b1, b->b2};
00638     int i;
00639     
00640     for (i=0; i<4; i++) {
00641         int j, ok=0;
00642         for (j=0; j<4; j++) {
00643             if (keysa[i] == keysa[j]) {
00644                 ok = 1;
00645                 break;
00646             }
00647         }
00648         if (!ok)
00649             return -1;
00650     }
00651     
00652     return 0;
00653 }
00654 
00655 static void get_edgepairkey(edgepairkey *key, int a1, int a2, int b1, int b2)
00656 {
00657     key->a1 = a1;
00658     key->a2 = a2;
00659     key->b1 = b1;
00660     key->b2 = b2;
00661 }
00662 
00663 /*an immense amount of duplication goes on here. . .a major performance hit, I'm sure*/
00664 static CollPair* cloth_edge_collision ( ModifierData *md1, ModifierData *md2, 
00665                                         BVHTreeOverlap *overlap, CollPair *collpair,
00666                                         GHash *visithash, MemArena *arena)
00667 {
00668     ClothModifierData *clmd = ( ClothModifierData * ) md1;
00669     CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
00670     MFace *face1=NULL, *face2 = NULL;
00671     ClothVertex *verts1 = clmd->clothObject->verts;
00672     double distance = 0;
00673     edgepairkey *key, tstkey;
00674     float epsilon1 = clmd->coll_parms->epsilon;
00675     float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
00676     float no[3], uv[3], t, relnor;
00677     int i, i1, i2, i3, i4, i5, i6;
00678     Cloth *cloth = clmd->clothObject;
00679     float n1[3], n2[3], off[3], v1[2][3], v2[2][3], v3[2][3], v4[2][3], v5[2][3], v6[2][3];
00680     void **verts[] = {v1, v2, v3, v4, v5, v6};
00681     int j, ret, bp1, bp2, bp3, ap1, ap2, ap3, table[6];
00682     
00683     face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
00684     face2 = & ( collmd->mfaces[overlap->indexB] );
00685 
00686     // check all 4 possible collisions
00687     for ( i = 0; i < 4; i++ )
00688     {
00689         if ( i == 0 )
00690         {
00691             // fill faceA
00692             ap1 = face1->v1;
00693             ap2 = face1->v2;
00694             ap3 = face1->v3;
00695 
00696             // fill faceB
00697             bp1 = face2->v1;
00698             bp2 = face2->v2;
00699             bp3 = face2->v3;
00700         }
00701         else if ( i == 1 )
00702         {
00703             if ( face1->v4 )
00704             {
00705                 // fill faceA
00706                 ap1 = face1->v1;
00707                 ap2 = face1->v3;
00708                 ap3 = face1->v4;
00709 
00710                 // fill faceB
00711                 bp1 = face2->v1;
00712                 bp2 = face2->v2;
00713                 bp3 = face2->v3;
00714             }
00715             else {
00716                 continue;
00717             }
00718         }
00719         if ( i == 2 )
00720         {
00721             if ( face2->v4 )
00722             {
00723                 // fill faceA
00724                 ap1 = face1->v1;
00725                 ap2 = face1->v2;
00726                 ap3 = face1->v3;
00727 
00728                 // fill faceB
00729                 bp1 = face2->v1;
00730                 bp2 = face2->v3;
00731                 bp3 = face2->v4;
00732             }
00733             else {
00734                 continue;
00735             }
00736         }
00737         else if ( i == 3 )
00738         {
00739             if ( face1->v4 && face2->v4 )
00740             {
00741                 // fill faceA
00742                 ap1 = face1->v1;
00743                 ap2 = face1->v3;
00744                 ap3 = face1->v4;
00745 
00746                 // fill faceB
00747                 bp1 = face2->v1;
00748                 bp2 = face2->v3;
00749                 bp3 = face2->v4;
00750             }
00751             else {
00752                 continue;
00753             }
00754         }
00755         
00756         copy_v3_v3(v1[0], cloth->verts[ap1].txold); 
00757         copy_v3_v3(v1[1], cloth->verts[ap1].tx);
00758         copy_v3_v3(v2[0], cloth->verts[ap2].txold);
00759         copy_v3_v3(v2[1], cloth->verts[ap2].tx);
00760         copy_v3_v3(v3[0], cloth->verts[ap3].txold);
00761         copy_v3_v3(v3[1], cloth->verts[ap3].tx);
00762         
00763         copy_v3_v3(v4[0], collmd->current_x[bp1].co);
00764         copy_v3_v3(v4[1], collmd->current_xnew[bp1].co);
00765         copy_v3_v3(v5[0], collmd->current_x[bp2].co);
00766         copy_v3_v3(v5[1], collmd->current_xnew[bp2].co);
00767         copy_v3_v3(v6[0], collmd->current_x[bp3].co);
00768         copy_v3_v3(v6[1], collmd->current_xnew[bp3].co);
00769         
00770         normal_tri_v3(n2, v4[1], v5[1], v6[1]);
00771 
00772         /*offset new positions a bit, to account for margins*/
00773         i1 = ap1; i2 = ap2; i3 = ap3;
00774         i4 = bp1; i5 = bp2; i6 = bp3;
00775 
00776         for (j=0; j<3; j++) {
00777             int collp1, collp2, k, j2 = (j+1)%3;
00778             
00779             table[0] = ap1; table[1] = ap2; table[2] = ap3;
00780             table[3] = bp1; table[4] = bp2; table[5] = bp3;
00781             for (k=0; k<3; k++) {
00782                 float p1[3], p2[3];
00783                 int k2 = (k+1)%3;
00784                 
00785                 get_edgepairkey(&tstkey, table[j], table[j2], table[k+3], table[k2+3]);
00786                 //if (BLI_ghash_haskey(visithash, &tstkey))
00787                 //  continue;
00788                 
00789                 key = BLI_memarena_alloc(arena, sizeof(edgepairkey));
00790                 *key = tstkey;
00791                 BLI_ghash_insert(visithash, key, NULL);
00792 
00793                 sub_v3_v3v3(p1, verts[j], verts[j2]);
00794                 sub_v3_v3v3(p2, verts[k+3], verts[k2+3]);
00795                 
00796                 cross_v3_v3v3(off, p1, p2);
00797                 normalize_v3(off);
00798 
00799                 if (dot_v3v3(n2, off) < 0.0)
00800                     negate_v3(off);
00801                 
00802                 mul_v3_fl(off,  epsilon1 + epsilon2 + ALMOST_ZERO);
00803                 copy_v3_v3(p1, verts[k+3]);
00804                 copy_v3_v3(p2, verts[k2+3]);
00805                 add_v3_v3(p1, off);
00806                 add_v3_v3(p2, off);
00807                 
00808                 ret = eltopo_line_line_moving_isect_v3v3_f(verts[j], table[j], verts[j2], table[j2], 
00809                                                            p1, table[k+3], p2, table[k2+3], 
00810                                                            no, uv, &t, &relnor);
00811                 /*cloth vert versus coll face*/
00812                 if (ret) {
00813                     collpair->ap1 = table[j]; collpair->ap2 = table[j2]; 
00814                     collpair->bp1 = table[k+3]; collpair->bp2 = table[k2+3];
00815                     
00816                     /*I'm not sure if this is correct, but hopefully it's 
00817                       better then simply ignoring back edges*/
00818                     if (dot_v3v3(n2, no) < 0.0) {
00819                         negate_v3(no);
00820                     }
00821                     
00822                     copy_v3_v3(collpair->normal, no);
00823                     mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
00824                     collpair->distance = relnor;
00825                     collpair->time = t;
00826                     
00827                     copy_v2_v2(collpair->bary, uv);
00828                     
00829                     collpair->flag = COLLISION_IS_EDGES;
00830                     collpair++;
00831                 }
00832             }
00833         }
00834     }
00835     
00836     return collpair;
00837 }
00838 
00839 static int cloth_edge_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
00840 {
00841     int result = 0;
00842     Cloth *cloth1;
00843     float w1, w2;
00844     float v1[3], v2[3], relativeVelocity[3];
00845     float magrelVel, pimpulse[3];
00846 
00847     cloth1 = clmd->clothObject;
00848 
00849     for ( ; collpair != collision_end; collpair++ )
00850     {
00851         if (!(collpair->flag & COLLISION_IS_EDGES))
00852             continue;
00853         
00854         // was: txold
00855         w1 = collpair->bary[0]; w2 = collpair->bary[1];         
00856         
00857         // Calculate relative "velocity".
00858         VECADDFAC(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, w1);
00859         VECADDFAC(v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, w2);
00860         
00861         VECSUB ( relativeVelocity, v2, v1);
00862         
00863         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
00864         magrelVel = INPR ( relativeVelocity, collpair->normal );
00865 
00866         // If v_n_mag < 0 the edges are approaching each other.
00867         if ( magrelVel > ALMOST_ZERO )
00868         {
00869             // Calculate Impulse magnitude to stop all motion in normal direction.
00870             float magtangent = 0, repulse = 0, d = 0;
00871             double impulse = 0.0;
00872             float vrel_t_pre[3];
00873             float temp[3], spf;
00874             
00875             zero_v3(pimpulse);
00876             
00877             // calculate tangential velocity
00878             VECCOPY ( temp, collpair->normal );
00879             mul_v3_fl( temp, magrelVel );
00880             VECSUB ( vrel_t_pre, relativeVelocity, temp );
00881 
00882             // Decrease in magnitude of relative tangential velocity due to coulomb friction
00883             // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
00884             magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
00885 
00886             // Apply friction impulse.
00887             if ( magtangent > ALMOST_ZERO )
00888             {
00889                 normalize_v3( vrel_t_pre );
00890 
00891                 impulse = magtangent; 
00892                 VECADDMUL ( pimpulse, vrel_t_pre, impulse);
00893             }
00894 
00895             // Apply velocity stopping impulse
00896             // I_c = m * v_N / 2.0
00897             // no 2.0 * magrelVel normally, but looks nicer DG
00898             impulse =  magrelVel;
00899             
00900             mul_v3_fl(collpair->normal, 0.5);
00901             VECADDMUL ( pimpulse, collpair->normal, impulse);
00902 
00903             // Apply repulse impulse if distance too short
00904             // I_r = -min(dt*kd, m(0,1d/dt - v_n))
00905             spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
00906 
00907             d = collpair->distance;
00908             if ( ( magrelVel < 0.1*d*spf && ( d > ALMOST_ZERO ) ) )
00909             {
00910                 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
00911 
00912                 // stay on the safe side and clamp repulse
00913                 if ( impulse > ALMOST_ZERO )
00914                     repulse = MIN2 ( repulse, 5.0*impulse );
00915                 repulse = MAX2 ( impulse, repulse );
00916 
00917                 impulse = repulse / ( 5.0 ); // original 2.0 / 0.25
00918                 VECADDMUL ( pimpulse, collpair->normal, impulse);
00919             }
00920             
00921             w2 = 1.0f-w1;
00922             if (w1 < 0.5)
00923                 w1 *= 2.0;
00924             else
00925                 w2 *= 2.0;
00926             
00927             VECADDFAC(cloth1->verts[collpair->ap1].impulse, cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0);
00928             VECADDFAC(cloth1->verts[collpair->ap2].impulse, cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0);
00929             
00930             cloth1->verts[collpair->ap1].impulse_count++;
00931             cloth1->verts[collpair->ap2].impulse_count++;
00932             
00933             result = 1;
00934         }
00935     } 
00936     
00937     return result;
00938 }
00939 
00940 static int cloth_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
00941 {
00942     int result = 0;
00943     Cloth *cloth1;
00944     float w1, w2, w3, u1, u2, u3;
00945     float v1[3], v2[3], relativeVelocity[3];
00946     float magrelVel;
00947     float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
00948     
00949     cloth1 = clmd->clothObject;
00950 
00951     for ( ; collpair != collision_end; collpair++ )
00952     {
00953         if (collpair->flag & COLLISION_IS_EDGES)
00954             continue;
00955         
00956         if ( collpair->flag & COLLISION_USE_COLLFACE ) {
00957             // was: txold
00958             w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2];         
00959 
00960             // Calculate relative "velocity".
00961             collision_interpolateOnTriangle ( v1, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, w1, w2, w3);
00962             
00963             VECSUB ( relativeVelocity, v1, cloth1->verts[collpair->collp].tv);
00964             
00965             // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
00966             magrelVel = INPR ( relativeVelocity, collpair->normal );
00967     
00968             // If v_n_mag < 0 the edges are approaching each other.
00969             if ( magrelVel > ALMOST_ZERO )
00970             {
00971                 // Calculate Impulse magnitude to stop all motion in normal direction.
00972                 float magtangent = 0, repulse = 0, d = 0;
00973                 double impulse = 0.0;
00974                 float vrel_t_pre[3];
00975                 float temp[3], spf;
00976     
00977                 // calculate tangential velocity
00978                 VECCOPY ( temp, collpair->normal );
00979                 mul_v3_fl( temp, magrelVel );
00980                 VECSUB ( vrel_t_pre, relativeVelocity, temp );
00981     
00982                 // Decrease in magnitude of relative tangential velocity due to coulomb friction
00983                 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
00984                 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
00985     
00986                 // Apply friction impulse.
00987                 if ( magtangent > ALMOST_ZERO )
00988                 {
00989                     normalize_v3( vrel_t_pre );
00990     
00991                     impulse = magtangent; // 2.0 * 
00992                     VECADDMUL ( cloth1->verts[collpair->collp].impulse, vrel_t_pre, impulse);
00993                 }
00994     
00995                 // Apply velocity stopping impulse
00996                 // I_c = m * v_N / 2.0
00997                 // no 2.0 * magrelVel normally, but looks nicer DG
00998                 impulse =  magrelVel/2.0;
00999     
01000                 VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse);
01001                 cloth1->verts[collpair->collp].impulse_count++;
01002     
01003                 // Apply repulse impulse if distance too short
01004                 // I_r = -min(dt*kd, m(0,1d/dt - v_n))
01005                 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
01006     
01007                 d = -collpair->distance;
01008                 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
01009                 {
01010                     repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
01011     
01012                     // stay on the safe side and clamp repulse
01013                     if ( impulse > ALMOST_ZERO )
01014                         repulse = MIN2 ( repulse, 5.0*impulse );
01015                     repulse = MAX2 ( impulse, repulse );
01016     
01017                     impulse = repulse / ( 5.0 ); // original 2.0 / 0.25
01018                     VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse);
01019                 }
01020     
01021                 result = 1;
01022             }
01023         } else {    
01024             w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2];         
01025 
01026             // Calculate relative "velocity".
01027             collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
01028     
01029             VECSUB ( relativeVelocity, collmd->current_v[collpair->collp].co, v1);
01030             
01031             // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
01032             magrelVel = INPR ( relativeVelocity, collpair->normal );
01033     
01034             // If v_n_mag < 0 the edges are approaching each other.
01035             if ( magrelVel > ALMOST_ZERO )
01036             {
01037                 // Calculate Impulse magnitude to stop all motion in normal direction.
01038                 float magtangent = 0, repulse = 0, d = 0;
01039                 double impulse = 0.0;
01040                 float vrel_t_pre[3], pimpulse[3] = {0.0f, 0.0f, 0.0f};
01041                 float temp[3], spf;
01042     
01043                 // calculate tangential velocity
01044                 VECCOPY ( temp, collpair->normal );
01045                 mul_v3_fl( temp, magrelVel );
01046                 VECSUB ( vrel_t_pre, relativeVelocity, temp );
01047     
01048                 // Decrease in magnitude of relative tangential velocity due to coulomb friction
01049                 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
01050                 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
01051     
01052                 // Apply friction impulse.
01053                 if ( magtangent > ALMOST_ZERO )
01054                 {
01055                     normalize_v3( vrel_t_pre );
01056     
01057                     impulse = magtangent; // 2.0 * 
01058                     VECADDMUL ( pimpulse, vrel_t_pre, impulse);
01059                 }
01060     
01061                 // Apply velocity stopping impulse
01062                 // I_c = m * v_N / 2.0
01063                 // no 2.0 * magrelVel normally, but looks nicer DG
01064                 impulse =  magrelVel/2.0;
01065     
01066                 VECADDMUL ( pimpulse, collpair->normal, impulse);
01067     
01068                 // Apply repulse impulse if distance too short
01069                 // I_r = -min(dt*kd, m(0,1d/dt - v_n))
01070                 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
01071     
01072                 d = -collpair->distance;
01073                 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
01074                 {
01075                     repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
01076     
01077                     // stay on the safe side and clamp repulse
01078                     if ( impulse > ALMOST_ZERO )
01079                         repulse = MIN2 ( repulse, 5.0*impulse );
01080                     repulse = MAX2 ( impulse, repulse );
01081     
01082                     impulse = repulse / ( 2.0 ); // original 2.0 / 0.25
01083                     VECADDMUL ( pimpulse, collpair->normal, impulse);
01084                 }
01085                 
01086                 if (w1 < 0.5) w1 *= 2.0;
01087                 if (w2 < 0.5) w2 *= 2.0;
01088                 if (w3 < 0.5) w3 *= 2.0;
01089                 
01090                 VECADDMUL(cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0);
01091                 VECADDMUL(cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0);
01092                 VECADDMUL(cloth1->verts[collpair->ap3].impulse, pimpulse, w3*2.0);
01093                 cloth1->verts[collpair->ap1].impulse_count++;
01094                 cloth1->verts[collpair->ap2].impulse_count++;
01095                 cloth1->verts[collpair->ap3].impulse_count++;
01096                 
01097                 result = 1;
01098             }
01099         }
01100     } 
01101     
01102     return result;
01103 }
01104 
01105 
01106 typedef struct tripairkey {
01107     int p, a1, a2, a3;
01108 } tripairkey;
01109 
01110 unsigned int tripair_hash(void *vkey)
01111 {
01112     tripairkey *key = vkey;
01113     int keys[4] = {key->p, key->a1, key->a2, key->a3};
01114     int i, j;
01115     
01116     for (i=0; i<4; i++) {
01117         for (j=0; j<3; j++) {
01118             if (keys[j] >= keys[j+1]) {
01119                 SWAP(int, keys[j], keys[j+1]);
01120             }
01121         }
01122     }
01123     
01124     return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34;
01125 }
01126 
01127 int tripair_cmp(const void *va, const void *vb)
01128 {
01129     tripairkey *a = va, *b = vb;
01130     int keysa[4] = {a->p, a->a1, a->a2, a->a3};
01131     int keysb[4] = {b->p, b->a1, b->a2, b->a3};
01132     int i;
01133     
01134     for (i=0; i<4; i++) {
01135         int j, ok=0;
01136         for (j=0; j<4; j++) {
01137             if (keysa[i] == keysa[j]) {
01138                 ok = 1;
01139                 break;
01140             }
01141         }
01142         if (!ok)
01143             return -1;
01144     }
01145     
01146     return 0;
01147 }
01148 
01149 static void get_tripairkey(tripairkey *key, int p, int a1, int a2, int a3)
01150 {
01151     key->a1 = a1;
01152     key->a2 = a2;
01153     key->a3 = a3;
01154     key->p = p;
01155 }
01156 
01157 static int checkvisit(MemArena *arena, GHash *gh, int p, int a1, int a2, int a3)
01158 {
01159     tripairkey key, *key2;
01160     
01161     get_tripairkey(&key, p, a1, a2, a3);
01162     if (BLI_ghash_haskey(gh, &key))
01163         return 1;
01164     
01165     key2 = BLI_memarena_alloc(arena, sizeof(*key2));
01166     *key2 = key;
01167     BLI_ghash_insert(gh, key2, NULL);
01168     
01169     return 0;
01170 }
01171 
01172 int cloth_point_tri_moving_v3v3_f(float v1[2][3], int i1, float v2[2][3], int i2,
01173                                    float v3[2][3],  int i3, float v4[2][3], int i4,
01174                                    float normal[3], float bary[3], float *t, 
01175                                    float *relnor, GHash *gh, MemArena *arena)
01176 {
01177     if (checkvisit(arena, gh, i1, i2, i3, i4))
01178         return 0;
01179     
01180     return eltopo_point_tri_moving_v3v3_f(v1, i1, v2, i2, v3, i3, v4, i4, normal, bary, t, relnor);
01181 }
01182 
01183 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, 
01184                                    CollPair *collpair, double dt, GHash *gh, MemArena *arena)
01185 {
01186     ClothModifierData *clmd = ( ClothModifierData * ) md1;
01187     CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
01188     MFace *face1=NULL, *face2 = NULL;
01189     ClothVertex *verts1 = clmd->clothObject->verts;
01190     double distance = 0;
01191     float epsilon1 = clmd->coll_parms->epsilon;
01192     float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
01193     float no[3], uv[3], t, relnor;
01194     int i, i1, i2, i3, i4, i5, i6;
01195     Cloth *cloth = clmd->clothObject;
01196     float n1[3], sdis, p[3], l, n2[3], off[3], v1[2][3], v2[2][3], v3[2][3], v4[2][3], v5[2][3], v6[2][3];
01197     int j, ret, bp1, bp2, bp3, ap1, ap2, ap3;
01198     
01199     face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
01200     face2 = & ( collmd->mfaces[overlap->indexB] );
01201 
01202     // check all 4 possible collisions
01203     for ( i = 0; i < 4; i++ )
01204     {
01205         if ( i == 0 )
01206         {
01207             // fill faceA
01208             ap1 = face1->v1;
01209             ap2 = face1->v2;
01210             ap3 = face1->v3;
01211 
01212             // fill faceB
01213             bp1 = face2->v1;
01214             bp2 = face2->v2;
01215             bp3 = face2->v3;
01216         }
01217         else if ( i == 1 )
01218         {
01219             if ( face1->v4 )
01220             {
01221                 // fill faceA
01222                 ap1 = face1->v1;
01223                 ap2 = face1->v3;
01224                 ap3 = face1->v4;
01225 
01226                 // fill faceB
01227                 bp1 = face2->v1;
01228                 bp2 = face2->v2;
01229                 bp3 = face2->v3;
01230             }
01231             else {
01232                 continue;
01233             }
01234         }
01235         if ( i == 2 )
01236         {
01237             if ( face2->v4 )
01238             {
01239                 // fill faceA
01240                 ap1 = face1->v1;
01241                 ap2 = face1->v2;
01242                 ap3 = face1->v3;
01243 
01244                 // fill faceB
01245                 bp1 = face2->v1;
01246                 bp2 = face2->v3;
01247                 bp3 = face2->v4;
01248             }
01249             else {
01250                 continue;
01251             }
01252         }
01253         else if ( i == 3 )
01254         {
01255             if ( face1->v4 && face2->v4 )
01256             {
01257                 // fill faceA
01258                 ap1 = face1->v1;
01259                 ap2 = face1->v3;
01260                 ap3 = face1->v4;
01261 
01262                 // fill faceB
01263                 bp1 = face2->v1;
01264                 bp2 = face2->v3;
01265                 bp3 = face2->v4;
01266             }
01267             else {
01268                 continue;
01269             }
01270         }
01271         
01272         copy_v3_v3(v1[0], cloth->verts[ap1].txold); 
01273         copy_v3_v3(v1[1], cloth->verts[ap1].tx);
01274         copy_v3_v3(v2[0], cloth->verts[ap2].txold);
01275         copy_v3_v3(v2[1], cloth->verts[ap2].tx);
01276         copy_v3_v3(v3[0], cloth->verts[ap3].txold);
01277         copy_v3_v3(v3[1], cloth->verts[ap3].tx);
01278         
01279         copy_v3_v3(v4[0], collmd->current_x[bp1].co);
01280         copy_v3_v3(v4[1], collmd->current_xnew[bp1].co);
01281         copy_v3_v3(v5[0], collmd->current_x[bp2].co);
01282         copy_v3_v3(v5[1], collmd->current_xnew[bp2].co);
01283         copy_v3_v3(v6[0], collmd->current_x[bp3].co);
01284         copy_v3_v3(v6[1], collmd->current_xnew[bp3].co);
01285         
01286         normal_tri_v3(n2, v4[1], v5[1], v6[1]);
01287         
01288         sdis = clmd->coll_parms->distance_repel + epsilon2 + FLT_EPSILON;
01289 
01290         /*apply a repulsion force, to help the solver along*/
01291         copy_v3_v3(off, n2);
01292         negate_v3(off);
01293         if (isect_ray_plane_v3(v1[1], off, v4[1], v5[1], v6[1], &l, 0)) {
01294             if (l >= 0.0 && l < sdis) {
01295                 mul_v3_fl(off, (l-sdis)*cloth->verts[ap1].mass*dt*clmd->coll_parms->repel_force*0.1);
01296 
01297                 add_v3_v3(cloth->verts[ap1].tv, off);
01298                 add_v3_v3(cloth->verts[ap2].tv, off);
01299                 add_v3_v3(cloth->verts[ap3].tv, off);
01300             }
01301         }
01302 
01303         /*offset new positions a bit, to account for margins*/
01304         copy_v3_v3(off, n2);
01305         mul_v3_fl(off,  epsilon1 + epsilon2 + ALMOST_ZERO);
01306         add_v3_v3(v4[1], off); add_v3_v3(v5[1], off); add_v3_v3(v6[1], off);
01307         
01308         i1 = ap1; i2 = ap2; i3 = ap3;
01309         i4 = bp1+cloth->numverts; i5 = bp2+cloth->numverts; i6 = bp3+cloth->numverts;
01310         
01311         for (j=0; j<6; j++) {
01312             int collp;
01313 
01314             switch (j) {
01315             case 0:
01316                 ret = cloth_point_tri_moving_v3v3_f(v1, i1, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
01317                 collp = ap1;
01318                 break;
01319             case 1:
01320                 collp = ap2;
01321                 ret = cloth_point_tri_moving_v3v3_f(v2, i2, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
01322                 break;
01323             case 2:
01324                 collp = ap3;
01325                 ret = cloth_point_tri_moving_v3v3_f(v3, i3, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
01326                 break;
01327             case 3:
01328                 collp = bp1;
01329                 ret = cloth_point_tri_moving_v3v3_f(v4, i4, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
01330                 break;
01331             case 4:
01332                 collp = bp2;                
01333                 ret = cloth_point_tri_moving_v3v3_f(v5, i5, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
01334                 break;
01335             case 5:
01336                 collp = bp3;
01337                 ret = cloth_point_tri_moving_v3v3_f(v6, i6, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
01338                 break;
01339             }
01340             
01341             /*cloth vert versus coll face*/
01342             if (ret && j < 3) {
01343                 collpair->bp1 = bp1; collpair->bp2 = bp2; collpair->bp3 = bp3;
01344                 collpair->collp = collp;
01345                 
01346                 copy_v3_v3(collpair->normal, no);
01347                 mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
01348                 collpair->distance = relnor;
01349                 collpair->time = t;
01350                 
01351                 copy_v3_v3(collpair->bary, uv);
01352                 
01353                 collpair->flag = COLLISION_USE_COLLFACE;
01354                 collpair++;
01355             } else if (ret && j >= 3) { /*coll vert versus cloth face*/
01356                 collpair->ap1 = ap1; collpair->ap2 = ap2; collpair->ap3 = ap3;
01357                 collpair->collp = collp;
01358                 
01359                 copy_v3_v3(collpair->normal, no);
01360                 mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
01361                 collpair->distance = relnor;
01362                 collpair->time = t;
01363                 
01364                 copy_v3_v3(collpair->bary, uv);
01365 
01366                 collpair->flag = 0;
01367                 collpair++;
01368             }
01369         }
01370     }
01371     
01372     return collpair;
01373 }
01374 
01375 static void machine_epsilon_offset(Cloth *cloth)
01376 {
01377     ClothVertex *cv;
01378     int i, j;
01379     
01380     cv = cloth->verts;
01381     for (i=0; i<cloth->numverts; i++, cv++) {
01382         /*aggrevatingly enough, it's necassary to offset the coordinates
01383          by a multiple of the 32-bit floating point epsilon when switching
01384          into doubles*/
01385         #define RNDSIGN (float)(-1*(BLI_rand()%2==0)|1)
01386         for (j=0; j<3; j++) {
01387             cv->tx[j] += FLT_EPSILON*30.0f*RNDSIGN;
01388             cv->txold[j] += FLT_EPSILON*30.0f*RNDSIGN;
01389             cv->tv[j] += FLT_EPSILON*30.0f*RNDSIGN;
01390         }       
01391     }
01392 }
01393 
01394 #else /* !WITH_ELTOPO */
01395 
01396 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
01397 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, 
01398     BVHTreeOverlap *overlap, CollPair *collpair, float dt )
01399 {
01400     ClothModifierData *clmd = ( ClothModifierData * ) md1;
01401     CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
01402     Cloth *cloth = clmd->clothObject;
01403     MFace *face1=NULL, *face2 = NULL;
01404 #ifdef USE_BULLET
01405     ClothVertex *verts1 = clmd->clothObject->verts;
01406 #endif
01407     double distance = 0;
01408     float epsilon1 = clmd->coll_parms->epsilon;
01409     float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
01410     float n2[3], sdis, l;
01411     int i;
01412 
01413     face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
01414     face2 = & ( collmd->mfaces[overlap->indexB] );
01415 
01416     // check all 4 possible collisions
01417     for ( i = 0; i < 4; i++ )
01418     {
01419         if ( i == 0 )
01420         {
01421             // fill faceA
01422             collpair->ap1 = face1->v1;
01423             collpair->ap2 = face1->v2;
01424             collpair->ap3 = face1->v3;
01425 
01426             // fill faceB
01427             collpair->bp1 = face2->v1;
01428             collpair->bp2 = face2->v2;
01429             collpair->bp3 = face2->v3;
01430         }
01431         else if ( i == 1 )
01432         {
01433             if ( face1->v4 )
01434             {
01435                 // fill faceA
01436                 collpair->ap1 = face1->v1;
01437                 collpair->ap2 = face1->v4;
01438                 collpair->ap3 = face1->v3;
01439 
01440                 // fill faceB
01441                 collpair->bp1 = face2->v1;
01442                 collpair->bp2 = face2->v2;
01443                 collpair->bp3 = face2->v3;
01444             }
01445             else
01446                 i++;
01447         }
01448         if ( i == 2 )
01449         {
01450             if ( face2->v4 )
01451             {
01452                 // fill faceA
01453                 collpair->ap1 = face1->v1;
01454                 collpair->ap2 = face1->v2;
01455                 collpair->ap3 = face1->v3;
01456 
01457                 // fill faceB
01458                 collpair->bp1 = face2->v1;
01459                 collpair->bp2 = face2->v4;
01460                 collpair->bp3 = face2->v3;
01461             }
01462             else
01463                 break;
01464         }
01465         else if ( i == 3 )
01466         {
01467             if ( face1->v4 && face2->v4 )
01468             {
01469                 // fill faceA
01470                 collpair->ap1 = face1->v1;
01471                 collpair->ap2 = face1->v4;
01472                 collpair->ap3 = face1->v3;
01473 
01474                 // fill faceB
01475                 collpair->bp1 = face2->v1;
01476                 collpair->bp2 = face2->v4;
01477                 collpair->bp3 = face2->v3;
01478             }
01479             else
01480                 break;
01481         }
01482         
01483         normal_tri_v3(n2, collmd->current_xnew[collpair->bp1].co, 
01484             collmd->current_xnew[collpair->bp2].co, 
01485             collmd->current_xnew[collpair->bp3].co);
01486         
01487         sdis = clmd->coll_parms->distance_repel + epsilon2 + FLT_EPSILON;
01488         
01489         /* apply a repulsion force, to help the solver along.
01490          * this is kindof crude, it only tests one vert of the triangle */
01491         if (isect_ray_plane_v3(cloth->verts[collpair->ap1].tx, n2, collmd->current_xnew[collpair->bp1].co, 
01492             collmd->current_xnew[collpair->bp2].co,
01493             collmd->current_xnew[collpair->bp3].co, &l, 0))
01494         {
01495             if (l >= 0.0f && l < sdis) {
01496                 mul_v3_fl(n2, (l-sdis)*cloth->verts[collpair->ap1].mass*dt*clmd->coll_parms->repel_force*0.1f);
01497 
01498                 add_v3_v3(cloth->verts[collpair->ap1].tv, n2);
01499                 add_v3_v3(cloth->verts[collpair->ap2].tv, n2);
01500                 add_v3_v3(cloth->verts[collpair->ap3].tv, n2);
01501             }
01502         }
01503         
01504 #ifdef USE_BULLET
01505         // calc distance + normal
01506         distance = plNearestPoints (
01507             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 );
01508 #else
01509         // just be sure that we don't add anything
01510         distance = 2.0 * (double)( epsilon1 + epsilon2 + ALMOST_ZERO );
01511 #endif
01512 
01513         if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
01514         {
01515             normalize_v3_v3( collpair->normal, collpair->vector );
01516 
01517             collpair->distance = distance;
01518             collpair->flag = 0;
01519             collpair++;
01520         }/*
01521         else
01522         {
01523             float w1, w2, w3, u1, u2, u3;
01524             float v1[3], v2[3], relativeVelocity[3];
01525 
01526             // calc relative velocity
01527             
01528             // compute barycentric coordinates for both collision points
01529             collision_compute_barycentric ( collpair->pa,
01530             verts1[collpair->ap1].txold,
01531             verts1[collpair->ap2].txold,
01532             verts1[collpair->ap3].txold,
01533             &w1, &w2, &w3 );
01534 
01535             // was: txold
01536             collision_compute_barycentric ( collpair->pb,
01537             collmd->current_x[collpair->bp1].co,
01538             collmd->current_x[collpair->bp2].co,
01539             collmd->current_x[collpair->bp3].co,
01540             &u1, &u2, &u3 );
01541 
01542             // Calculate relative "velocity".
01543             collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
01544 
01545             collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
01546 
01547             VECSUB ( relativeVelocity, v2, v1 );
01548 
01549             if(sqrt(INPR(relativeVelocity, relativeVelocity)) >= distance)
01550             {
01551                 // check for collision in the future
01552                 collpair->flag |= COLLISION_IN_FUTURE;
01553                 collpair++;
01554             }
01555         }*/
01556     }
01557     return collpair;
01558 }
01559 #endif /* WITH_ELTOPO */
01560 
01561 
01562 #if 0
01563 static int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
01564 {
01565     int result = 0;
01566     Cloth *cloth1;
01567     float w1, w2, w3, u1, u2, u3;
01568     float v1[3], v2[3], relativeVelocity[3];
01569     float magrelVel;
01570 
01571     cloth1 = clmd->clothObject;
01572 
01573     for ( ; collpair != collision_end; collpair++ )
01574     {
01575         // compute barycentric coordinates for both collision points
01576         collision_compute_barycentric ( collpair->pa,
01577             cloth1->verts[collpair->ap1].txold,
01578             cloth1->verts[collpair->ap2].txold,
01579             cloth1->verts[collpair->ap3].txold,
01580             &w1, &w2, &w3 );
01581 
01582         // was: txold
01583         collision_compute_barycentric ( collpair->pb,
01584             collmd->current_x[collpair->bp1].co,
01585             collmd->current_x[collpair->bp2].co,
01586             collmd->current_x[collpair->bp3].co,
01587             &u1, &u2, &u3 );
01588 
01589         // Calculate relative "velocity".
01590         collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
01591 
01592         collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
01593 
01594         VECSUB ( relativeVelocity, v2, v1 );
01595 
01596         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
01597         magrelVel = INPR ( relativeVelocity, collpair->normal );
01598 
01599         // printf("magrelVel: %f\n", magrelVel);
01600 
01601         // Calculate masses of points.
01602         // TODO
01603 
01604         // If v_n_mag < 0 the edges are approaching each other.
01605         if ( magrelVel > ALMOST_ZERO )
01606         {
01607             // Calculate Impulse magnitude to stop all motion in normal direction.
01608             float magtangent = 0;
01609             double impulse = 0.0;
01610             float vrel_t_pre[3];
01611             float temp[3];
01612 
01613             // calculate tangential velocity
01614             VECCOPY ( temp, collpair->normal );
01615             mul_v3_fl( temp, magrelVel );
01616             VECSUB ( vrel_t_pre, relativeVelocity, temp );
01617 
01618             // Decrease in magnitude of relative tangential velocity due to coulomb friction
01619             // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
01620             magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
01621 
01622             // Apply friction impulse.
01623             if ( magtangent > ALMOST_ZERO )
01624             {
01625                 normalize_v3( vrel_t_pre );
01626 
01627                 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
01628                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
01629                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
01630                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
01631             }
01632 
01633             // Apply velocity stopping impulse
01634             // I_c = m * v_N / 2.0
01635             // no 2.0 * magrelVel normally, but looks nicer DG
01636             impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
01637 
01638             VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
01639             cloth1->verts[collpair->ap1].impulse_count++;
01640 
01641             VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
01642             cloth1->verts[collpair->ap2].impulse_count++;
01643 
01644             VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
01645             cloth1->verts[collpair->ap3].impulse_count++;
01646 
01647             // Apply repulse impulse if distance too short
01648             // I_r = -min(dt*kd, m(0,1d/dt - v_n))
01649             /*
01650             d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
01651             if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
01652             {
01653             repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
01654 
01655             // stay on the safe side and clamp repulse
01656             if ( impulse > ALMOST_ZERO )
01657             repulse = MIN2 ( repulse, 5.0*impulse );
01658             repulse = MAX2 ( impulse, repulse );
01659 
01660             impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
01661             VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
01662             VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
01663             VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
01664             }
01665             */
01666             result = 1;
01667         }
01668     }
01669     return result;
01670 }
01671 #endif
01672 
01673 #if 0
01674 static float projectPointOntoLine(float *p, float *a, float *b) 
01675 {
01676     float ba[3], pa[3];
01677     VECSUB(ba, b, a);
01678     VECSUB(pa, p, a);
01679     return INPR(pa, ba) / INPR(ba, ba);
01680 }
01681 
01682 static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal) 
01683 {
01684     float line1[3], line2[3];
01685     float length;
01686 
01687     VECSUB(line1, np2, np1);
01688     VECSUB(line2, np3, np1);
01689 
01690     // printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]);
01691 
01692     cross_v3_v3v3(out_normal, line1, line2);
01693 
01694     
01695 
01696     length = normalize_v3(out_normal);
01697     if (length <= FLT_EPSILON)
01698     { // lines are collinear
01699         VECSUB(out_normal, np2, np1);
01700         normalize_v3(out_normal);
01701     }
01702 }
01703 
01704 static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2)
01705 {
01706     float temp[3], temp2[3];
01707     
01708     double a, b, c, e, f; 
01709 
01710     VECSUB(temp, x2, x1);
01711     a = INPR(temp, temp);
01712 
01713     VECSUB(temp2, x4, x3);
01714     b = -INPR(temp, temp2);
01715 
01716     c = INPR(temp2, temp2);
01717 
01718     VECSUB(temp2, x3, x1);
01719     e = INPR(temp, temp2);
01720 
01721     VECSUB(temp, x4, x3);
01722     f = -INPR(temp, temp2);
01723 
01724     *w1 = (e * c - b * f) / (a * c - b * b);
01725     *w2 = (f - b * *w1) / c;
01726 
01727 }
01728 
01729 // calculates the distance of 2 edges
01730 static float edgedge_distance(float np11[3], float np12[3], float np21[3], float np22[3], float *out_a1, float *out_a2, float *out_normal)
01731 {
01732     float line1[3], line2[3], cross[3];
01733     float length;
01734     float temp[3], temp2[3];
01735     float dist_a1, dist_a2;
01736     
01737     VECSUB(line1, np12, np11);
01738     VECSUB(line2, np22, np21);
01739 
01740     cross_v3_v3v3(cross, line1, line2);
01741     length = INPR(cross, cross);
01742 
01743     if (length < FLT_EPSILON) 
01744     {
01745         *out_a2 = projectPointOntoLine(np11, np21, np22);
01746         if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON)) 
01747         {
01748             *out_a1 = 0;
01749             calculateEENormal(np11, np12, np21, np22, out_normal);
01750             VECSUB(temp, np22, np21);
01751             mul_v3_fl(temp, *out_a2);
01752             VECADD(temp2, temp, np21);
01753             VECADD(temp2, temp2, np11);
01754             return INPR(temp2, temp2);
01755         }
01756 
01757         CLAMP(*out_a2, 0.0, 1.0);
01758         if (*out_a2 > .5) 
01759         { // == 1.0
01760             *out_a1 = projectPointOntoLine(np22, np11, np12);
01761             if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 
01762             {
01763                 calculateEENormal(np11, np12, np21, np22, out_normal);
01764 
01765                 // return (np22 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
01766                 VECSUB(temp, np12, np11);
01767                 mul_v3_fl(temp, *out_a1);
01768                 VECADD(temp2, temp, np11);
01769                 VECSUB(temp2, np22, temp2);
01770                 return INPR(temp2, temp2);
01771             }
01772         } 
01773         else 
01774         { // == 0.0
01775             *out_a1 = projectPointOntoLine(np21, np11, np12);
01776             if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 
01777             {
01778                 calculateEENormal(np11, np11, np21, np22, out_normal);
01779 
01780                 // return (np21 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
01781                 VECSUB(temp, np12, np11);
01782                 mul_v3_fl(temp, *out_a1);
01783                 VECADD(temp2, temp, np11);
01784                 VECSUB(temp2, np21, temp2);
01785                 return INPR(temp2, temp2);
01786             }
01787         }
01788 
01789         CLAMP(*out_a1, 0.0, 1.0);
01790         calculateEENormal(np11, np12, np21, np22, out_normal);
01791         if(*out_a1 > .5)
01792         {
01793             if(*out_a2 > .5)
01794             {
01795                 VECSUB(temp, np12, np22);
01796             }
01797             else
01798             {
01799                 VECSUB(temp, np12, np21);
01800             }
01801         }
01802         else
01803         {
01804             if(*out_a2 > .5)
01805             {
01806                 VECSUB(temp, np11, np22);
01807             }
01808             else
01809             {
01810                 VECSUB(temp, np11, np21);
01811             }
01812         }
01813 
01814         return INPR(temp, temp);
01815     }
01816     else
01817     {
01818         
01819         // If the lines aren't parallel (but coplanar) they have to intersect
01820 
01821         findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2);
01822 
01823         // If both points are on the finite edges, we're done.
01824         if (*out_a1 >= 0.0 && *out_a1 <= 1.0 && *out_a2 >= 0.0 && *out_a2 <= 1.0) 
01825         {
01826             float p1[3], p2[3];
01827             
01828             // p1= np11 + (np12 - np11) * out_a1;
01829             VECSUB(temp, np12, np11);
01830             mul_v3_fl(temp, *out_a1);
01831             VECADD(p1, np11, temp);
01832             
01833             // p2 = np21 + (np22 - np21) * out_a2;
01834             VECSUB(temp, np22, np21);
01835             mul_v3_fl(temp, *out_a2);
01836             VECADD(p2, np21, temp);
01837 
01838             calculateEENormal(np11, np12, np21, np22, out_normal);
01839             VECSUB(temp, p1, p2);
01840             return INPR(temp, temp);
01841         }
01842 
01843         
01844         /*
01845         * Clamp both points to the finite edges.
01846         * The one that moves most during clamping is one part of the solution.
01847         */
01848         dist_a1 = *out_a1;
01849         CLAMP(dist_a1, 0.0, 1.0);
01850         dist_a2 = *out_a2;
01851         CLAMP(dist_a2, 0.0, 1.0);
01852 
01853         // Now project the "most clamped" point on the other line.
01854         if (dist_a1 > dist_a2) 
01855         { 
01856             /* keep out_a1 */
01857             float p1[3];
01858 
01859             // p1 = np11 + (np12 - np11) * out_a1;
01860             VECSUB(temp, np12, np11);
01861             mul_v3_fl(temp, *out_a1);
01862             VECADD(p1, np11, temp);
01863 
01864             *out_a2 = projectPointOntoLine(p1, np21, np22);
01865             CLAMP(*out_a2, 0.0, 1.0);
01866 
01867             calculateEENormal(np11, np12, np21, np22, out_normal);
01868 
01869             // return (p1 - (np21 + (np22 - np21) * out_a2)).lengthSquared();
01870             VECSUB(temp, np22, np21);
01871             mul_v3_fl(temp, *out_a2);
01872             VECADD(temp, temp, np21);
01873             VECSUB(temp, p1, temp);
01874             return INPR(temp, temp);
01875         } 
01876         else 
01877         {   
01878             /* keep out_a2 */
01879             float p2[3];
01880             
01881             // p2 = np21 + (np22 - np21) * out_a2;
01882             VECSUB(temp, np22, np21);
01883             mul_v3_fl(temp, *out_a2);
01884             VECADD(p2, np21, temp);
01885 
01886             *out_a1 = projectPointOntoLine(p2, np11, np12);
01887             CLAMP(*out_a1, 0.0, 1.0);
01888 
01889             calculateEENormal(np11, np12, np21, np22, out_normal);
01890             
01891             // return ((np11 + (np12 - np11) * out_a1) - p2).lengthSquared();
01892             VECSUB(temp, np12, np11);
01893             mul_v3_fl(temp, *out_a1);
01894             VECADD(temp, temp, np11);
01895             VECSUB(temp, temp, p2);
01896             return INPR(temp, temp);
01897         }
01898     }
01899     
01900     printf("Error in edgedge_distance: end of function\n");
01901     return 0;
01902 }
01903 
01904 static int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
01905 {
01906     EdgeCollPair edgecollpair;
01907     Cloth *cloth1=NULL;
01908     ClothVertex *verts1=NULL;
01909     unsigned int i = 0, k = 0;
01910     int numsolutions = 0;
01911     double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
01912     double solution[3], solution2[3];
01913     MVert *verts2 = collmd->current_x; // old x
01914     MVert *velocity2 = collmd->current_v; // velocity
01915     float distance = 0;
01916     float triA[3][3], triB[3][3];
01917     int result = 0;
01918 
01919     cloth1 = clmd->clothObject;
01920     verts1 = cloth1->verts;
01921 
01922     for(i = 0; i < 9; i++)
01923     {
01924         // 9 edge - edge possibilities
01925 
01926         if(i == 0) // cloth edge: 1-2; coll edge: 1-2
01927         {
01928             edgecollpair.p11 = collpair->ap1;
01929             edgecollpair.p12 = collpair->ap2;
01930 
01931             edgecollpair.p21 = collpair->bp1;
01932             edgecollpair.p22 = collpair->bp2;
01933         }
01934         else if(i == 1) // cloth edge: 1-2; coll edge: 2-3
01935         {
01936             edgecollpair.p11 = collpair->ap1;
01937             edgecollpair.p12 = collpair->ap2;
01938 
01939             edgecollpair.p21 = collpair->bp2;
01940             edgecollpair.p22 = collpair->bp3;
01941         }
01942         else if(i == 2) // cloth edge: 1-2; coll edge: 1-3
01943         {
01944             edgecollpair.p11 = collpair->ap1;
01945             edgecollpair.p12 = collpair->ap2;
01946 
01947             edgecollpair.p21 = collpair->bp1;
01948             edgecollpair.p22 = collpair->bp3;
01949         }
01950         else if(i == 3) // cloth edge: 2-3; coll edge: 1-2
01951         {
01952             edgecollpair.p11 = collpair->ap2;
01953             edgecollpair.p12 = collpair->ap3;
01954 
01955             edgecollpair.p21 = collpair->bp1;
01956             edgecollpair.p22 = collpair->bp2;
01957         }
01958         else if(i == 4) // cloth edge: 2-3; coll edge: 2-3
01959         {
01960             edgecollpair.p11 = collpair->ap2;
01961             edgecollpair.p12 = collpair->ap3;
01962 
01963             edgecollpair.p21 = collpair->bp2;
01964             edgecollpair.p22 = collpair->bp3;
01965         }
01966         else if(i == 5) // cloth edge: 2-3; coll edge: 1-3
01967         {
01968             edgecollpair.p11 = collpair->ap2;
01969             edgecollpair.p12 = collpair->ap3;
01970 
01971             edgecollpair.p21 = collpair->bp1;
01972             edgecollpair.p22 = collpair->bp3;
01973         }
01974         else if(i ==6) // cloth edge: 1-3; coll edge: 1-2
01975         {
01976             edgecollpair.p11 = collpair->ap1;
01977             edgecollpair.p12 = collpair->ap3;
01978 
01979             edgecollpair.p21 = collpair->bp1;
01980             edgecollpair.p22 = collpair->bp2;
01981         }
01982         else if(i ==7) // cloth edge: 1-3; coll edge: 2-3
01983         {
01984             edgecollpair.p11 = collpair->ap1;
01985             edgecollpair.p12 = collpair->ap3;
01986 
01987             edgecollpair.p21 = collpair->bp2;
01988             edgecollpair.p22 = collpair->bp3;
01989         }
01990         else if(i == 8) // cloth edge: 1-3; coll edge: 1-3
01991         {
01992             edgecollpair.p11 = collpair->ap1;
01993             edgecollpair.p12 = collpair->ap3;
01994 
01995             edgecollpair.p21 = collpair->bp1;
01996             edgecollpair.p22 = collpair->bp3;
01997         }
01998         /*
01999         if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16))
02000             printf("Ahier!\n");
02001         if((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3))
02002             printf("Ahier!\n");
02003         */
02004 
02005         // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
02006         {
02007             // always put coll points in p21/p22
02008             VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
02009             VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
02010 
02011             VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
02012             VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
02013 
02014             VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold );
02015             VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv );
02016 
02017             numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
02018 
02019             if((edgecollpair.p11 == 3 && edgecollpair.p12==16)|| (edgecollpair.p11==16 && edgecollpair.p12==3))
02020             {
02021                 if(edgecollpair.p21==6 || edgecollpair.p22 == 6)
02022                 {
02023                     printf("dist: %f, sol[k]: %f, sol2[k]: %f\n", distance, solution[k], solution2[k]);
02024                     printf("a1: %f, a2: %f, b1: %f, b2: %f\n", x1[0], x2[0], x3[0], v1[0]);
02025                     printf("b21: %d, b22: %d\n", edgecollpair.p21, edgecollpair.p22);
02026                 }
02027             }
02028 
02029             for ( k = 0; k < numsolutions; k++ )
02030             {
02031                 // printf("sol %d: %lf\n", k, solution[k]);
02032                 if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) && ( solution[k] >  ALMOST_ZERO))
02033                 {
02034                     float a,b;
02035                     float out_normal[3];
02036                     float distance;
02037                     float impulse = 0;
02038                     float I_mag;
02039 
02040                     // move verts
02041                     VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, solution[k]);
02042                     VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, solution[k]);
02043 
02044                     VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, solution[k]);
02045                     VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, solution[k]);
02046 
02047                     // TODO: check for collisions
02048                     distance = edgedge_distance(triA[0], triA[1], triB[0], triB[1], &a, &b, out_normal);
02049                     
02050                     if ((distance <= clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO) && (INPR(out_normal, out_normal) > 0))
02051                     {
02052                         float vrel_1_to_2[3], temp[3], temp2[3], out_normalVelocity;
02053                         float desiredVn;
02054 
02055                         VECCOPY(vrel_1_to_2, verts1[edgecollpair.p11].tv);
02056                         mul_v3_fl(vrel_1_to_2, 1.0 - a);
02057                         VECCOPY(temp, verts1[edgecollpair.p12].tv);
02058                         mul_v3_fl(temp, a);
02059 
02060                         VECADD(vrel_1_to_2, vrel_1_to_2, temp);
02061 
02062                         VECCOPY(temp, verts1[edgecollpair.p21].tv);
02063                         mul_v3_fl(temp, 1.0 - b);
02064                         VECCOPY(temp2, verts1[edgecollpair.p22].tv);
02065                         mul_v3_fl(temp2, b);
02066                         VECADD(temp, temp, temp2);
02067 
02068                         VECSUB(vrel_1_to_2, vrel_1_to_2, temp);
02069 
02070                         out_normalVelocity = INPR(vrel_1_to_2, out_normal);
02071 /*
02072                         // this correction results in wrong normals sometimes?
02073                         if(out_normalVelocity < 0.0)
02074                         {
02075                             out_normalVelocity*= -1.0;
02076                             negate_v3(out_normal);
02077                         }
02078 */
02079                         /* Inelastic repulsion impulse. */
02080 
02081                         // Calculate which normal velocity we need. 
02082                         desiredVn = (out_normalVelocity * (float)solution[k] - (.1 * (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree )) - sqrt(distance)) - ALMOST_ZERO);
02083 
02084                         // Now calculate what impulse we need to reach that velocity. 
02085                         I_mag = (out_normalVelocity - desiredVn) / 2.0; // / (1/m1 + 1/m2);
02086 
02087                         // Finally apply that impulse. 
02088                         impulse = (2.0 * -I_mag) / (a*a + (1.0-a)*(1.0-a) + b*b + (1.0-b)*(1.0-b));
02089 
02090                         VECADDMUL ( verts1[edgecollpair.p11].impulse, out_normal, (1.0-a) * impulse );
02091                         verts1[edgecollpair.p11].impulse_count++;
02092 
02093                         VECADDMUL ( verts1[edgecollpair.p12].impulse, out_normal, a * impulse );
02094                         verts1[edgecollpair.p12].impulse_count++;
02095 
02096                         // return true;
02097                         result = 1;
02098                         break;
02099                     }
02100                     else
02101                     {
02102                         // missing from collision.hpp
02103                     }
02104                     // mintime = MIN2(mintime, (float)solution[k]);
02105 
02106                     break;
02107                 }
02108             }
02109         }
02110     }
02111     return result;
02112 }
02113 
02114 static int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
02115 {
02116     Cloth *cloth1;
02117     cloth1 = clmd->clothObject;
02118 
02119     for ( ; collpair != collision_end; collpair++ )
02120     {
02121         // only handle moving collisions here
02122         if (!( collpair->flag & COLLISION_IN_FUTURE ))
02123             continue;
02124 
02125         cloth_collision_moving_edges ( clmd, collmd, collpair);
02126         // cloth_collision_moving_tris ( clmd, collmd, collpair);
02127     }
02128 
02129     return 1;
02130 }
02131 #endif
02132 
02133 static void add_collision_object(Object ***objs, unsigned int *numobj, unsigned int *maxobj, Object *ob, Object *self, int level)
02134 {
02135     CollisionModifierData *cmd= NULL;
02136 
02137     if(ob == self)
02138         return;
02139 
02140     /* only get objects with collision modifier */
02141     if(ob->pd && ob->pd->deflect)
02142         cmd= (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
02143     
02144     if(cmd) {   
02145         /* extend array */
02146         if(*numobj >= *maxobj) {
02147             *maxobj *= 2;
02148             *objs= MEM_reallocN(*objs, sizeof(Object*)*(*maxobj));
02149         }
02150         
02151         (*objs)[*numobj] = ob;
02152         (*numobj)++;
02153     }
02154 
02155     /* objects in dupli groups, one level only for now */
02156     if(ob->dup_group && level == 0) {
02157         GroupObject *go;
02158         Group *group= ob->dup_group;
02159 
02160         /* add objects */
02161         for(go= group->gobject.first; go; go= go->next)
02162             add_collision_object(objs, numobj, maxobj, go->ob, self, level+1);
02163     }   
02164 }
02165 
02166 // return all collision objects in scene
02167 // collision object will exclude self 
02168 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj)
02169 {
02170     Base *base;
02171     Object **objs;
02172     GroupObject *go;
02173     unsigned int numobj= 0, maxobj= 100;
02174     
02175     objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
02176 
02177     /* gather all collision objects */
02178     if(group) {
02179         /* use specified group */
02180         for(go= group->gobject.first; go; go= go->next)
02181             add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0);
02182     }
02183     else {
02184         Scene *sce_iter;
02185         /* add objects in same layer in scene */
02186         for(SETLOOPER(scene, sce_iter, base)) {
02187             if(base->lay & self->lay)
02188                 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0);
02189 
02190         }
02191     }
02192 
02193     *numcollobj= numobj;
02194 
02195     return objs;
02196 }
02197 
02198 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
02199 {
02200     CollisionModifierData *cmd= NULL;
02201     ColliderCache *col;
02202 
02203     if(ob == self)
02204         return;
02205 
02206     if(ob->pd && ob->pd->deflect)
02207         cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
02208     
02209     if(cmd && cmd->bvhtree) {   
02210         if(*objs == NULL)
02211             *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
02212 
02213         col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
02214         col->ob = ob;
02215         col->collmd = cmd;
02216         /* make sure collider is properly set up */
02217         collision_move_object(cmd, 1.0, 0.0);
02218         BLI_addtail(*objs, col);
02219     }
02220 
02221     /* objects in dupli groups, one level only for now */
02222     if(ob->dup_group && level == 0) {
02223         GroupObject *go;
02224         Group *group= ob->dup_group;
02225 
02226         /* add objects */
02227         for(go= group->gobject.first; go; go= go->next)
02228             add_collider_cache_object(objs, go->ob, self, level+1);
02229     }
02230 }
02231 
02232 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
02233 {
02234     GroupObject *go;
02235     ListBase *objs= NULL;
02236     
02237     /* add object in same layer in scene */
02238     if(group) {
02239         for(go= group->gobject.first; go; go= go->next)
02240             add_collider_cache_object(&objs, go->ob, self, 0);
02241     }
02242     else {
02243         Scene *sce_iter;
02244         Base *base;
02245 
02246         /* add objects in same layer in scene */
02247         for(SETLOOPER(scene, sce_iter, base)) {
02248             if(!self || (base->lay & self->lay))
02249                 add_collider_cache_object(&objs, base->object, self, 0);
02250 
02251         }
02252     }
02253 
02254     return objs;
02255 }
02256 
02257 void free_collider_cache(ListBase **colliders)
02258 {
02259     if(*colliders) {
02260         BLI_freelistN(*colliders);
02261         MEM_freeN(*colliders);
02262         *colliders = NULL;
02263     }
02264 }
02265 
02266 
02267 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd,
02268     CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap, double dt)
02269 {
02270     int i;
02271 #ifdef WITH_ELTOPO
02272     GHash *visithash = BLI_ghash_new(edgepair_hash, edgepair_cmp, "visthash, collision.c");
02273     GHash *tri_visithash = BLI_ghash_new(tripair_hash, tripair_cmp, "tri_visthash, collision.c");
02274     MemArena *arena = BLI_memarena_new(1<<16, "edge hash arena, collision.c");
02275 #endif
02276     
02277     *collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * numresult * 64, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
02278     *collisions_index = *collisions;
02279     
02280 #ifdef WITH_ELTOPO
02281     machine_epsilon_offset(clmd->clothObject);
02282 
02283     for ( i = 0; i < numresult; i++ )
02284     {
02285         *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd,
02286                                               overlap+i, *collisions_index, dt, tri_visithash, arena );
02287     }
02288 
02289     for ( i = 0; i < numresult; i++ )
02290     {
02291         *collisions_index = cloth_edge_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd,
02292                                                    overlap+i, *collisions_index, visithash, arena );
02293     }
02294     BLI_ghash_free(visithash, NULL, NULL);
02295     BLI_ghash_free(tri_visithash, NULL, NULL);
02296     BLI_memarena_free(arena);
02297 #else /* WITH_ELTOPO */
02298     for ( i = 0; i < numresult; i++ )
02299     {
02300         *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd,
02301                                               overlap+i, *collisions_index, dt );
02302     }
02303 #endif /* WITH_ELTOPO */
02304 
02305 }
02306 
02307 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
02308 {
02309     Cloth *cloth = clmd->clothObject;
02310     int i=0, j = 0, /*numfaces = 0,*/ numverts = 0;
02311     ClothVertex *verts = NULL;
02312     int ret = 0;
02313     int result = 0;
02314     float tnull[3] = {0,0,0};
02315     
02316     /*numfaces = clmd->clothObject->numfaces;*/ /*UNUSED*/
02317     numverts = clmd->clothObject->numverts;
02318  
02319     verts = cloth->verts;
02320     
02321     // process all collisions (calculate impulses, TODO: also repulses if distance too short)
02322     result = 1;
02323     for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence
02324     {
02325         result = 0;
02326 
02327         if ( collmd->bvhtree )
02328         {
02329 #ifdef WITH_ELTOPO
02330             result += cloth_collision_response_moving(clmd, collmd, collisions, collisions_index);
02331             result += cloth_edge_collision_response_moving(clmd, collmd, collisions, collisions_index);
02332 #else
02333             result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
02334 #endif
02335 #ifdef WITH_ELTOPO
02336             {
02337 #else
02338             // apply impulses in parallel
02339             if ( result )
02340             {
02341 #endif
02342                 for ( i = 0; i < numverts; i++ )
02343                 {
02344                     // calculate "velocities" (just xnew = xold + v; no dt in v)
02345                     if ( verts[i].impulse_count )
02346                     {
02347                         VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
02348                         copy_v3_v3 ( verts[i].impulse, tnull );
02349                         verts[i].impulse_count = 0;
02350 
02351                         ret++;
02352                     }
02353                 }
02354             }
02355         }
02356     }
02357     return ret;
02358 }
02359 
02360 // cloth - object collisions
02361 int cloth_bvh_objcollision (Object *ob, ClothModifierData * clmd, float step, float dt )
02362 {
02363     Cloth *cloth= clmd->clothObject;
02364     BVHTree *cloth_bvh= cloth->bvhtree;
02365     unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ numverts = 0, k, l, j;
02366     int rounds = 0; // result counts applied collisions; ic is for debug output;
02367     ClothVertex *verts = NULL;
02368     int ret = 0, ret2 = 0;
02369     Object **collobjs = NULL;
02370     unsigned int numcollobj = 0;
02371 
02372     if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
02373         return 0;
02374     
02375     verts = cloth->verts;
02376     /* numfaces = cloth->numfaces; */ /* UNUSED */
02377     numverts = cloth->numverts;
02378 
02380     // static collisions
02382 
02383     // update cloth bvh
02384     bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
02385     bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
02386     
02387     collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj);
02388     
02389     if(!collobjs)
02390         return 0;
02391 
02392     do
02393     {
02394         CollPair **collisions, **collisions_index;
02395         
02396         ret2 = 0;
02397 
02398         collisions = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
02399         collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
02400         
02401         // check all collision objects
02402         for(i = 0; i < numcollobj; i++)
02403         {
02404             Object *collob= collobjs[i];
02405             CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision);
02406             BVHTreeOverlap *overlap = NULL;
02407             unsigned int result = 0;
02408             
02409             if(!collmd->bvhtree)
02410                 continue;
02411             
02412             /* move object to position (step) in time */
02413             
02414             collision_move_object ( collmd, step + dt, step );
02415             
02416             /* search for overlapping collision pairs */
02417             overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
02418                 
02419             // go to next object if no overlap is there
02420             if( result && overlap ) {
02421                 /* check if collisions really happen (costly near check) */
02422                 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], 
02423                     &collisions_index[i], result, overlap, dt/(float)clmd->coll_parms->loop_count);
02424             
02425                 // resolve nearby collisions
02426                 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
02427                 ret2 += ret;
02428             }
02429 
02430             if ( overlap )
02431                 MEM_freeN ( overlap );
02432         }
02433         rounds++;
02434         
02435         for(i = 0; i < numcollobj; i++)
02436         {
02437             if ( collisions[i] ) MEM_freeN ( collisions[i] );
02438         }
02439             
02440         MEM_freeN(collisions);
02441         MEM_freeN(collisions_index);
02442 
02444         // update positions
02445         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
02447 
02448         // verts come from clmd
02449         for ( i = 0; i < numverts; i++ )
02450         {
02451             if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
02452             {
02453                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED )
02454                 {
02455                     continue;
02456                 }
02457             }
02458 
02459             VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
02460         }
02462         
02463         
02465         // Test on *simple* selfcollisions
02467         if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
02468         {
02469             for(l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++)
02470             {
02471                 // TODO: add coll quality rounds again
02472                 BVHTreeOverlap *overlap = NULL;
02473                 unsigned int result = 0;
02474     
02475                 // collisions = 1;
02476                 verts = cloth->verts; // needed for openMP
02477     
02478                 /* numfaces = cloth->numfaces; */ /* UNUSED */
02479                 numverts = cloth->numverts;
02480     
02481                 verts = cloth->verts;
02482     
02483                 if ( cloth->bvhselftree )
02484                 {
02485                     // search for overlapping collision pairs 
02486                     overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
02487     
02488     // #pragma omp parallel for private(k, i, j) schedule(static)
02489                     for ( k = 0; k < result; k++ )
02490                     {
02491                         float temp[3];
02492                         float length = 0;
02493                         float mindistance;
02494     
02495                         i = overlap[k].indexA;
02496                         j = overlap[k].indexB;
02497     
02498                         mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
02499     
02500                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
02501                         {
02502                             if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
02503                                         && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
02504                             {
02505                                 continue;
02506                             }
02507                         }
02508     
02509                         VECSUB ( temp, verts[i].tx, verts[j].tx );
02510     
02511                         if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
02512     
02513                         // check for adjacent points (i must be smaller j)
02514                         if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
02515                         {
02516                             continue;
02517                         }
02518     
02519                         length = normalize_v3( temp );
02520     
02521                         if ( length < mindistance )
02522                         {
02523                             float correction = mindistance - length;
02524     
02525                             if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
02526                             {
02527                                 mul_v3_fl( temp, -correction );
02528                                 VECADD ( verts[j].tx, verts[j].tx, temp );
02529                             }
02530                             else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
02531                             {
02532                                 mul_v3_fl( temp, correction );
02533                                 VECADD ( verts[i].tx, verts[i].tx, temp );
02534                             }
02535                             else
02536                             {
02537                                 mul_v3_fl( temp, correction * -0.5 );
02538                                 VECADD ( verts[j].tx, verts[j].tx, temp );
02539     
02540                                 VECSUB ( verts[i].tx, verts[i].tx, temp );
02541                             }
02542                             ret = 1;
02543                             ret2 += ret;
02544                         }
02545                         else
02546                         {
02547                             // check for approximated time collisions
02548                         }
02549                     }
02550     
02551                     if ( overlap )
02552                         MEM_freeN ( overlap );
02553     
02554                 }
02555             }
02557 
02559             // SELFCOLLISIONS: update velocities
02561             if ( ret2 )
02562             {
02563                 for ( i = 0; i < cloth->numverts; i++ )
02564                 {
02565                     if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) )
02566                     {
02567                         VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold );
02568                     }
02569                 }
02570             }
02572         }
02573     }
02574     while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
02575     
02576     if(collobjs)
02577         MEM_freeN(collobjs);
02578 
02579     return 1|MIN2 ( ret, 1 );
02580 }