source/blender/blenkernel/intern/collision.c

   1 /*  collision.c
   2 *
   3 *
   4 * ***** BEGIN GPL LICENSE BLOCK *****
   5 *
   6 * This program is free software; you can redistribute it and/or
   7 * modify it under the terms of the GNU General Public License
   8 * as published by the Free Software Foundation; either version 2
   9 * of the License, or (at your option) any later version.
  10 *
  11 * This program is distributed in the hope that it will be useful,
  12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 * GNU General Public License for more details.
  15 *
  16 * You should have received a copy of the GNU General Public License
  17 * along with this program; if not, write to the Free Software Foundation,
  18 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  19 *
  20 * The Original Code is Copyright (C) Blender Foundation
  21 * All rights reserved.
  22 *
  23 * The Original Code is: all of this file.
  24 *
  25 * Contributor(s): none yet.
  26 *
  27 * ***** END GPL LICENSE BLOCK *****
  28 */
  29
  30 #include "MEM_guardedalloc.h"
  31
  32 #include "BKE_cloth.h"
  33
  34 #include "DNA_cloth_types.h"
  35 #include "DNA_group_types.h"
  36 #include "DNA_mesh_types.h"
  37 #include "DNA_object_types.h"
  38 #include "DNA_object_force.h"
  39 #include "DNA_scene_types.h"
  40 #include "DNA_meshdata_types.h"
  41
  42 #include "BLI_blenlib.h"
  43 #include "BLI_math.h"
  44 #include "BLI_edgehash.h"
  45
  46 #include "BKE_DerivedMesh.h"
  47 #include "BKE_global.h"
  48 #include "BKE_scene.h"
  49 #include "BKE_mesh.h"
  50 #include "BKE_object.h"
  51 #include "BKE_modifier.h"
  52 #include "BKE_utildefines.h"
  53 #include "BKE_DerivedMesh.h"
  54 #ifdef USE_BULLET
  55 #include "Bullet-C-Api.h"
  56 #endif
  57 #include "BLI_kdopbvh.h"
  58 #include "BKE_collision.h"
  59
  60
  61 /***********************************
  62 Collision modifier code start
  63 ***********************************/
  64
  65 /* step is limited from 0 (frame start position) to 1 (frame end position) */
  66 void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep )
  67 {
  68         float tv[3] = {0, 0, 0};
  69         unsigned int i = 0;
  70
  71         for ( i = 0; i < collmd->numverts; i++ )
  72         {
  73                 VECSUB ( tv, collmd->xnew[i].co, collmd->x[i].co );
  74                 VECADDS ( collmd->current_x[i].co, collmd->x[i].co, tv, prevstep );
  75                 VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step );
  76                 VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co );
  77         }
  78
  79         bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
  80 }
  81
  82 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
  83 {
  84         BVHTree *tree;
  85         float co[12];
  86         unsigned int i;
  87         MFace *tface = mfaces;
  88
  89         tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
  90
  91         // fill tree
  92         for ( i = 0; i < numfaces; i++, tface++ )
  93         {
  94                 VECCOPY ( &co[0*3], x[tface->v1].co );
  95                 VECCOPY ( &co[1*3], x[tface->v2].co );
  96                 VECCOPY ( &co[2*3], x[tface->v3].co );
  97                 if ( tface->v4 )
  98                         VECCOPY ( &co[3*3], x[tface->v4].co );
  99
 100                 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
 101         }
 102
 103         // balance tree
 104         BLI_bvhtree_balance ( tree );
 105
 106         return tree;
 107 }
 108
 109 void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
 110 {
 111         int i;
 112         MFace *mfaces = faces;
 113         float co[12], co_moving[12];
 114         int ret = 0;
 115
 116         if ( !bvhtree )
 117                 return;
 118
 119         if ( x )
 120         {
 121                 for ( i = 0; i < numfaces; i++, mfaces++ )
 122                 {
 123                         VECCOPY ( &co[0*3], x[mfaces->v1].co );
 124                         VECCOPY ( &co[1*3], x[mfaces->v2].co );
 125                         VECCOPY ( &co[2*3], x[mfaces->v3].co );
 126                         if ( mfaces->v4 )
 127                                 VECCOPY ( &co[3*3], x[mfaces->v4].co );
 128
 129                         // copy new locations into array
 130                         if ( moving && xnew )
 131                         {
 132                                 // update moving positions
 133                                 VECCOPY ( &co_moving[0*3], xnew[mfaces->v1].co );
 134                                 VECCOPY ( &co_moving[1*3], xnew[mfaces->v2].co );
 135                                 VECCOPY ( &co_moving[2*3], xnew[mfaces->v3].co );
 136                                 if ( mfaces->v4 )
 137                                         VECCOPY ( &co_moving[3*3], xnew[mfaces->v4].co );
 138
 139                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
 140                         }
 141                         else
 142                         {
 143                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
 144                         }
 145
 146                         // check if tree is already full
 147                         if ( !ret )
 148                                 break;
 149                 }
 150
 151                 BLI_bvhtree_update_tree ( bvhtree );
 152         }
 153 }
 154
 155 /***********************************
 156 Collision modifier code end
 157 ***********************************/
 158
 159 /**
 160 * gsl_poly_solve_cubic -
 161 *
 162 * copied from SOLVE_CUBIC.C --> GSL
 163 */
 164
 165 #define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0)
 166
 167 int
 168 gsl_poly_solve_cubic (double a, double b, double c,
 169                                           double *x0, double *x1, double *x2)
 170 {
 171         double q = (a * a - 3 * b);
 172         double r = (2 * a * a * a - 9 * a * b + 27 * c);
 173
 174         double Q = q / 9;
 175         double R = r / 54;
 176
 177         double Q3 = Q * Q * Q;
 178         double R2 = R * R;
 179
 180         double CR2 = 729 * r * r;
 181         double CQ3 = 2916 * q * q * q;
 182
 183         if (R == 0 && Q == 0)
 184         {
 185                 *x0 = - a / 3 ;
 186                 *x1 = - a / 3 ;
 187                 *x2 = - a / 3 ;
 188                 return 3 ;
 189         }
 190         else if (CR2 == CQ3)
 191         {
 192                 /* this test is actually R2 == Q3, written in a form suitable
 193                 for exact computation with integers */
 194
 195                 /* Due to finite precision some double roots may be missed, and
 196                 considered to be a pair of complex roots z = x +/- epsilon i
 197                 close to the real axis. */
 198
 199                 double sqrtQ = sqrt (Q);
 200
 201                 if (R > 0)
 202                 {
 203                         *x0 = -2 * sqrtQ  - a / 3;
 204                         *x1 = sqrtQ - a / 3;
 205                         *x2 = sqrtQ - a / 3;
 206                 }
 207                 else
 208                 {
 209                         *x0 = - sqrtQ  - a / 3;
 210                         *x1 = - sqrtQ - a / 3;
 211                         *x2 = 2 * sqrtQ - a / 3;
 212                 }
 213                 return 3 ;
 214         }
 215         else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
 216         {
 217                 double sqrtQ = sqrt (Q);
 218                 double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
 219                 double theta = acos (R / sqrtQ3);
 220                 double norm = -2 * sqrtQ;
 221                 *x0 = norm * cos (theta / 3) - a / 3;
 222                 *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
 223                 *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;
 224
 225                 /* Sort *x0, *x1, *x2 into increasing order */
 226
 227                 if (*x0 > *x1)
 228                         mySWAP(*x0, *x1) ;
 229
 230                 if (*x1 > *x2)
 231                 {
 232                         mySWAP(*x1, *x2) ;
 233
 234                         if (*x0 > *x1)
 235                                 mySWAP(*x0, *x1) ;
 236                 }
 237
 238                 return 3;
 239         }
 240         else
 241         {
 242                 double sgnR = (R >= 0 ? 1 : -1);
 243                 double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0);
 244                 double B = Q / A ;
 245                 *x0 = A + B - a / 3;
 246                 return 1;
 247         }
 248 }
 249
 250
 251
 252 /**
 253 * gsl_poly_solve_quadratic
 254 *
 255 * copied from GSL
 256 */
 257 int
 258 gsl_poly_solve_quadratic (double a, double b, double c,
 259                                                   double *x0, double *x1)
 260 {
 261         double disc = b * b - 4 * a * c;
 262
 263         if (a == 0) /* Handle linear case */
 264         {
 265                 if (b == 0)
 266                 {
 267                         return 0;
 268                 }
 269                 else
 270                 {
 271                         *x0 = -c / b;
 272                         return 1;
 273                 };
 274         }
 275
 276         if (disc > 0)
 277         {
 278                 if (b == 0)
 279                 {
 280                         double r = fabs (0.5 * sqrt (disc) / a);
 281                         *x0 = -r;
 282                         *x1 =  r;
 283                 }
 284                 else
 285                 {
 286                         double sgnb = (b > 0 ? 1 : -1);
 287                         double temp = -0.5 * (b + sgnb * sqrt (disc));
 288                         double r1 = temp / a ;
 289                         double r2 = c / temp ;
 290
 291                         if (r1 < r2)
 292                         {
 293                                 *x0 = r1 ;
 294                                 *x1 = r2 ;
 295                         }
 296                         else
 297                         {
 298                                 *x0 = r2 ;
 299                                 *x1 = r1 ;
 300                         }
 301                 }
 302                 return 2;
 303         }
 304         else if (disc == 0)
 305         {
 306                 *x0 = -0.5 * b / a ;
 307                 *x1 = -0.5 * b / a ;
 308                 return 2 ;
 309         }
 310         else
 311         {
 312                 return 0;
 313         }
 314 }
 315
 316
 317
 318
 319 /*
 320 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
 321 *     page 4, left column
 322 */
 323 #if 0
 324 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] )
 325 {
 326         int num_sols = 0;
 327
 328         // x^0 - checked
 329         double g =      a[0] * c[1] * e[2] - a[0] * c[2] * e[1] +
 330                 a[1] * c[2] * e[0] - a[1] * c[0] * e[2] +
 331                 a[2] * c[0] * e[1] - a[2] * c[1] * e[0];
 332
 333         // x^1
 334         double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
 335                 a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
 336                 a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
 337                 b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
 338                 a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
 339                 a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
 340
 341         // x^2
 342         double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
 343                 b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
 344                 b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
 345                 b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
 346                 a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
 347                 b[2] * c[0] * f[1] - b[0] * c[2] * f[1] +
 348                 a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
 349                 b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
 350                 a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
 351
 352         // x^3 - checked
 353         double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
 354                 b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
 355                 b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
 356
 357         /*
 358         printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]);
 359         printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]);
 360         printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]);
 361
 362         printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
 363         printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
 364         printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
 365
 366         printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
 367         printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
 368         printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
 369
 370         printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
 371
 372 */
 373         // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
 374         if ( ABS ( j ) > DBL_EPSILON )
 375         {
 376                 i /= j;
 377                 h /= j;
 378                 g /= j;
 379                 num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
 380         }
 381         else
 382         {
 383                 num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
 384                 solution[2] = -1.0;
 385         }
 386
 387         // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0],  solution[1],  solution[2]);
 388
 389         // Discard negative solutions
 390         if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) )
 391         {
 392                 --num_sols;
 393                 solution[0] = solution[num_sols];
 394         }
 395         if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) )
 396         {
 397                 --num_sols;
 398                 solution[1] = solution[num_sols];
 399         }
 400         if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) )
 401         {
 402                 --num_sols;
 403         }
 404
 405         // Sort
 406         if ( num_sols == 2 )
 407         {
 408                 if ( solution[0] > solution[1] )
 409                 {
 410                         double tmp = solution[0];
 411                         solution[0] = solution[1];
 412                         solution[1] = tmp;
 413                 }
 414         }
 415         else if ( num_sols == 3 )
 416         {
 417
 418                 // Bubblesort
 419                 if ( solution[0] > solution[1] )
 420                 {
 421                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
 422                 }
 423                 if ( solution[1] > solution[2] )
 424                 {
 425                         double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
 426                 }
 427                 if ( solution[0] > solution[1] )
 428                 {
 429                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
 430                 }
 431         }
 432
 433         return num_sols;
 434 }
 435 #endif
 436
 437
 438 // w3 is not perfect
 439 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
 440 {
 441         double  tempV1[3], tempV2[3], tempV4[3];
 442         double  a,b,c,d,e,f;
 443
 444         VECSUB ( tempV1, p1, p3 );
 445         VECSUB ( tempV2, p2, p3 );
 446         VECSUB ( tempV4, pv, p3 );
 447
 448         a = INPR ( tempV1, tempV1 );
 449         b = INPR ( tempV1, tempV2 );
 450         c = INPR ( tempV2, tempV2 );
 451         e = INPR ( tempV1, tempV4 );
 452         f = INPR ( tempV2, tempV4 );
 453
 454         d = ( a * c - b * b );
 455
 456         if ( ABS ( d ) < ALMOST_ZERO )
 457         {
 458                 *w1 = *w2 = *w3 = 1.0 / 3.0;
 459                 return;
 460         }
 461
 462         w1[0] = ( float ) ( ( e * c - b * f ) / d );
 463
 464         if ( w1[0] < 0 )
 465                 w1[0] = 0;
 466
 467         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
 468
 469         if ( w2[0] < 0 )
 470                 w2[0] = 0;
 471
 472         w3[0] = 1.0f - w1[0] - w2[0];
 473 }
 474
 475 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
 476 {
 477         to[0] = to[1] = to[2] = 0;
 478         VECADDMUL ( to, v1, w1 );
 479         VECADDMUL ( to, v2, w2 );
 480         VECADDMUL ( to, v3, w3 );
 481 }
 482
 483
 484 int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
 485 {
 486         int result = 0;
 487         Cloth *cloth1;
 488         float w1, w2, w3, u1, u2, u3;
 489         float v1[3], v2[3], relativeVelocity[3];
 490         float magrelVel;
 491         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
 492
 493         cloth1 = clmd->clothObject;
 494
 495         for ( ; collpair != collision_end; collpair++ )
 496         {
 497                 // only handle static collisions here
 498                 if ( collpair->flag & COLLISION_IN_FUTURE )
 499                         continue;
 500
 501                 // compute barycentric coordinates for both collision points
 502                 collision_compute_barycentric ( collpair->pa,
 503                         cloth1->verts[collpair->ap1].txold,
 504                         cloth1->verts[collpair->ap2].txold,
 505                         cloth1->verts[collpair->ap3].txold,
 506                         &w1, &w2, &w3 );
 507
 508                 // was: txold
 509                 collision_compute_barycentric ( collpair->pb,
 510                         collmd->current_x[collpair->bp1].co,
 511                         collmd->current_x[collpair->bp2].co,
 512                         collmd->current_x[collpair->bp3].co,
 513                         &u1, &u2, &u3 );
 514
 515                 // Calculate relative "velocity".
 516                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
 517
 518                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
 519
 520                 VECSUB ( relativeVelocity, v2, v1 );
 521
 522                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
 523                 magrelVel = INPR ( relativeVelocity, collpair->normal );
 524
 525                 // printf("magrelVel: %f\n", magrelVel);
 526
 527                 // Calculate masses of points.
 528                 // TODO
 529
 530                 // If v_n_mag < 0 the edges are approaching each other.
 531                 if ( magrelVel > ALMOST_ZERO )
 532                 {
 533                         // Calculate Impulse magnitude to stop all motion in normal direction.
 534                         float magtangent = 0, repulse = 0, d = 0;
 535                         double impulse = 0.0;
 536                         float vrel_t_pre[3];
 537                         float temp[3], spf;
 538
 539                         // calculate tangential velocity
 540                         VECCOPY ( temp, collpair->normal );
 541                         mul_v3_fl( temp, magrelVel );
 542                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
 543
 544                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
 545                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
 546                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
 547
 548                         // Apply friction impulse.
 549                         if ( magtangent > ALMOST_ZERO )
 550                         {
 551                                 normalize_v3( vrel_t_pre );
 552
 553                                 impulse = magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // 2.0 *
 554                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
 555                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
 556                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
 557                         }
 558
 559                         // Apply velocity stopping impulse
 560                         // I_c = m * v_N / 2.0
 561                         // no 2.0 * magrelVel normally, but looks nicer DG
 562                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
 563
 564                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
 565                         cloth1->verts[collpair->ap1].impulse_count++;
 566
 567                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
 568                         cloth1->verts[collpair->ap2].impulse_count++;
 569
 570                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
 571                         cloth1->verts[collpair->ap3].impulse_count++;
 572
 573                         // Apply repulse impulse if distance too short
 574                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
 575                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
 576
 577                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
 578                         if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
 579                         {
 580                                 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
 581
 582                                 // stay on the safe side and clamp repulse
 583                                 if ( impulse > ALMOST_ZERO )
 584                                         repulse = MIN2 ( repulse, 5.0*impulse );
 585                                 repulse = MAX2 ( impulse, repulse );
 586
 587                                 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
 588                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
 589                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
 590                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
 591                         }
 592
 593                         result = 1;
 594                 }
 595         }
 596         return result;
 597 }
 598
 599 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
 600 CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, CollPair *collpair )
 601 {
 602         ClothModifierData *clmd = ( ClothModifierData * ) md1;
 603         CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
 604         MFace *face1=NULL, *face2 = NULL;
 605 #ifdef USE_BULLET
 606         ClothVertex *verts1 = clmd->clothObject->verts;
 607 #endif
 608         double distance = 0;
 609         float epsilon1 = clmd->coll_parms->epsilon;
 610         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
 611         int i;
 612
 613         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
 614         face2 = & ( collmd->mfaces[overlap->indexB] );
 615
 616         // check all 4 possible collisions
 617         for ( i = 0; i < 4; i++ )
 618         {
 619                 if ( i == 0 )
 620                 {
 621                         // fill faceA
 622                         collpair->ap1 = face1->v1;
 623                         collpair->ap2 = face1->v2;
 624                         collpair->ap3 = face1->v3;
 625
 626                         // fill faceB
 627                         collpair->bp1 = face2->v1;
 628                         collpair->bp2 = face2->v2;
 629                         collpair->bp3 = face2->v3;
 630                 }
 631                 else if ( i == 1 )
 632                 {
 633                         if ( face1->v4 )
 634                         {
 635                                 // fill faceA
 636                                 collpair->ap1 = face1->v1;
 637                                 collpair->ap2 = face1->v4;
 638                                 collpair->ap3 = face1->v3;
 639
 640                                 // fill faceB
 641                                 collpair->bp1 = face2->v1;
 642                                 collpair->bp2 = face2->v2;
 643                                 collpair->bp3 = face2->v3;
 644                         }
 645                         else
 646                                 i++;
 647                 }
 648                 if ( i == 2 )
 649                 {
 650                         if ( face2->v4 )
 651                         {
 652                                 // fill faceA
 653                                 collpair->ap1 = face1->v1;
 654                                 collpair->ap2 = face1->v2;
 655                                 collpair->ap3 = face1->v3;
 656
 657                                 // fill faceB
 658                                 collpair->bp1 = face2->v1;
 659                                 collpair->bp2 = face2->v4;
 660                                 collpair->bp3 = face2->v3;
 661                         }
 662                         else
 663                                 break;
 664                 }
 665                 else if ( i == 3 )
 666                 {
 667                         if ( face1->v4 && face2->v4 )
 668                         {
 669                                 // fill faceA
 670                                 collpair->ap1 = face1->v1;
 671                                 collpair->ap2 = face1->v4;
 672                                 collpair->ap3 = face1->v3;
 673
 674                                 // fill faceB
 675                                 collpair->bp1 = face2->v1;
 676                                 collpair->bp2 = face2->v4;
 677                                 collpair->bp3 = face2->v3;
 678                         }
 679                         else
 680                                 break;
 681                 }
 682
 683 #ifdef USE_BULLET
 684                 // calc distance + normal
 685                 distance = plNearestPoints (
 686                         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 );
 687 #else
 688                 // just be sure that we don't add anything
 689                 distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO );
 690 #endif
 691
 692                 if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
 693                 {
 694                         normalize_v3_v3( collpair->normal, collpair->vector );
 695
 696                         collpair->distance = distance;
 697                         collpair->flag = 0;
 698                         collpair++;
 699                 }/*
 700                 else
 701                 {
 702                         float w1, w2, w3, u1, u2, u3;
 703                         float v1[3], v2[3], relativeVelocity[3];
 704
 705                         // calc relative velocity
 706
 707                         // compute barycentric coordinates for both collision points
 708                         collision_compute_barycentric ( collpair->pa,
 709                         verts1[collpair->ap1].txold,
 710                         verts1[collpair->ap2].txold,
 711                         verts1[collpair->ap3].txold,
 712                         &w1, &w2, &w3 );
 713
 714                         // was: txold
 715                         collision_compute_barycentric ( collpair->pb,
 716                         collmd->current_x[collpair->bp1].co,
 717                         collmd->current_x[collpair->bp2].co,
 718                         collmd->current_x[collpair->bp3].co,
 719                         &u1, &u2, &u3 );
 720
 721                         // Calculate relative "velocity".
 722                         collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
 723
 724                         collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
 725
 726                         VECSUB ( relativeVelocity, v2, v1 );
 727
 728                         if(sqrt(INPR(relativeVelocity, relativeVelocity)) >= distance)
 729                         {
 730                                 // check for collision in the future
 731                                 collpair->flag |= COLLISION_IN_FUTURE;
 732                                 collpair++;
 733                         }
 734                 }*/
 735         }
 736         return collpair;
 737 }
 738
 739 #if 0
 740 static int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
 741 {
 742         int result = 0;
 743         Cloth *cloth1;
 744         float w1, w2, w3, u1, u2, u3;
 745         float v1[3], v2[3], relativeVelocity[3];
 746         float magrelVel;
 747
 748         cloth1 = clmd->clothObject;
 749
 750         for ( ; collpair != collision_end; collpair++ )
 751         {
 752                 // compute barycentric coordinates for both collision points
 753                 collision_compute_barycentric ( collpair->pa,
 754                         cloth1->verts[collpair->ap1].txold,
 755                         cloth1->verts[collpair->ap2].txold,
 756                         cloth1->verts[collpair->ap3].txold,
 757                         &w1, &w2, &w3 );
 758
 759                 // was: txold
 760                 collision_compute_barycentric ( collpair->pb,
 761                         collmd->current_x[collpair->bp1].co,
 762                         collmd->current_x[collpair->bp2].co,
 763                         collmd->current_x[collpair->bp3].co,
 764                         &u1, &u2, &u3 );
 765
 766                 // Calculate relative "velocity".
 767                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
 768
 769                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
 770
 771                 VECSUB ( relativeVelocity, v2, v1 );
 772
 773                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
 774                 magrelVel = INPR ( relativeVelocity, collpair->normal );
 775
 776                 // printf("magrelVel: %f\n", magrelVel);
 777
 778                 // Calculate masses of points.
 779                 // TODO
 780
 781                 // If v_n_mag < 0 the edges are approaching each other.
 782                 if ( magrelVel > ALMOST_ZERO )
 783                 {
 784                         // Calculate Impulse magnitude to stop all motion in normal direction.
 785                         float magtangent = 0;
 786                         double impulse = 0.0;
 787                         float vrel_t_pre[3];
 788                         float temp[3];
 789
 790                         // calculate tangential velocity
 791                         VECCOPY ( temp, collpair->normal );
 792                         mul_v3_fl( temp, magrelVel );
 793                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
 794
 795                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
 796                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
 797                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
 798
 799                         // Apply friction impulse.
 800                         if ( magtangent > ALMOST_ZERO )
 801                         {
 802                                 normalize_v3( vrel_t_pre );
 803
 804                                 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
 805                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
 806                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
 807                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
 808                         }
 809
 810                         // Apply velocity stopping impulse
 811                         // I_c = m * v_N / 2.0
 812                         // no 2.0 * magrelVel normally, but looks nicer DG
 813                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
 814
 815                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
 816                         cloth1->verts[collpair->ap1].impulse_count++;
 817
 818                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
 819                         cloth1->verts[collpair->ap2].impulse_count++;
 820
 821                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
 822                         cloth1->verts[collpair->ap3].impulse_count++;
 823
 824                         // Apply repulse impulse if distance too short
 825                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
 826                         /*
 827                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
 828                         if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
 829                         {
 830                         repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
 831
 832                         // stay on the safe side and clamp repulse
 833                         if ( impulse > ALMOST_ZERO )
 834                         repulse = MIN2 ( repulse, 5.0*impulse );
 835                         repulse = MAX2 ( impulse, repulse );
 836
 837                         impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
 838                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
 839                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
 840                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
 841                         }
 842                         */
 843                         result = 1;
 844                 }
 845         }
 846         return result;
 847 }
 848 #endif
 849
 850 #if 0
 851 static float projectPointOntoLine(float *p, float *a, float *b)
 852 {
 853    float ba[3], pa[3];
 854    VECSUB(ba, b, a);
 855    VECSUB(pa, p, a);
 856    return INPR(pa, ba) / INPR(ba, ba);
 857 }
 858
 859 static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal)
 860 {
 861         float line1[3], line2[3];
 862         float length;
 863
 864         VECSUB(line1, np2, np1);
 865         VECSUB(line2, np3, np1);
 866
 867         // printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]);
 868
 869         cross_v3_v3v3(out_normal, line1, line2);
 870
 871
 872
 873         length = normalize_v3(out_normal);
 874         if (length <= FLT_EPSILON)
 875         { // lines are collinear
 876                 VECSUB(out_normal, np2, np1);
 877                 normalize_v3(out_normal);
 878         }
 879 }
 880
 881 static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2)
 882 {
 883         float temp[3], temp2[3];
 884
 885         double a, b, c, e, f;
 886
 887         VECSUB(temp, x2, x1);
 888         a = INPR(temp, temp);
 889
 890         VECSUB(temp2, x4, x3);
 891         b = -INPR(temp, temp2);
 892
 893         c = INPR(temp2, temp2);
 894
 895         VECSUB(temp2, x3, x1);
 896         e = INPR(temp, temp2);
 897
 898         VECSUB(temp, x4, x3);
 899         f = -INPR(temp, temp2);
 900
 901         *w1 = (e * c - b * f) / (a * c - b * b);
 902         *w2 = (f - b * *w1) / c;
 903
 904 }
 905
 906 // calculates the distance of 2 edges
 907 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)
 908 {
 909         float line1[3], line2[3], cross[3];
 910         float length;
 911         float temp[3], temp2[3];
 912         float dist_a1, dist_a2;
 913
 914         VECSUB(line1, np12, np11);
 915         VECSUB(line2, np22, np21);
 916
 917         cross_v3_v3v3(cross, line1, line2);
 918         length = INPR(cross, cross);
 919
 920         if (length < FLT_EPSILON)
 921         {
 922                 *out_a2 = projectPointOntoLine(np11, np21, np22);
 923                 if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON))
 924                 {
 925                         *out_a1 = 0;
 926                         calculateEENormal(np11, np12, np21, np22, out_normal);
 927                         VECSUB(temp, np22, np21);
 928                         mul_v3_fl(temp, *out_a2);
 929                         VECADD(temp2, temp, np21);
 930                         VECADD(temp2, temp2, np11);
 931                         return INPR(temp2, temp2);
 932                 }
 933
 934                 CLAMP(*out_a2, 0.0, 1.0);
 935                 if (*out_a2 > .5)
 936                 { // == 1.0
 937                         *out_a1 = projectPointOntoLine(np22, np11, np12);
 938                         if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON))
 939                         {
 940                                 calculateEENormal(np11, np12, np21, np22, out_normal);
 941
 942                                 // return (np22 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
 943                                 VECSUB(temp, np12, np11);
 944                                 mul_v3_fl(temp, *out_a1);
 945                                 VECADD(temp2, temp, np11);
 946                                 VECSUB(temp2, np22, temp2);
 947                                 return INPR(temp2, temp2);
 948                         }
 949                 }
 950                 else
 951                 { // == 0.0
 952                         *out_a1 = projectPointOntoLine(np21, np11, np12);
 953                         if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON))
 954                         {
 955                                 calculateEENormal(np11, np11, np21, np22, out_normal);
 956
 957                                 // return (np21 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
 958                                 VECSUB(temp, np12, np11);
 959                                 mul_v3_fl(temp, *out_a1);
 960                                 VECADD(temp2, temp, np11);
 961                                 VECSUB(temp2, np21, temp2);
 962                                 return INPR(temp2, temp2);
 963                         }
 964                 }
 965
 966                 CLAMP(*out_a1, 0.0, 1.0);
 967                 calculateEENormal(np11, np12, np21, np22, out_normal);
 968                 if(*out_a1 > .5)
 969                 {
 970                         if(*out_a2 > .5)
 971                         {
 972                                 VECSUB(temp, np12, np22);
 973                         }
 974                         else
 975                         {
 976                                 VECSUB(temp, np12, np21);
 977                         }
 978                 }
 979                 else
 980                 {
 981                         if(*out_a2 > .5)
 982                         {
 983                                 VECSUB(temp, np11, np22);
 984                         }
 985                         else
 986                         {
 987                                 VECSUB(temp, np11, np21);
 988                         }
 989                 }
 990
 991                 return INPR(temp, temp);
 992         }
 993         else
 994         {
 995
 996                 // If the lines aren't parallel (but coplanar) they have to intersect
 997
 998                 findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2);
 999
1000                 // If both points are on the finite edges, we're done.
1001                 if (*out_a1 >= 0.0 && *out_a1 <= 1.0 && *out_a2 >= 0.0 && *out_a2 <= 1.0)
1002                 {
1003                         float p1[3], p2[3];
1004
1005                         // p1= np11 + (np12 - np11) * out_a1;
1006                         VECSUB(temp, np12, np11);
1007                         mul_v3_fl(temp, *out_a1);
1008                         VECADD(p1, np11, temp);
1009
1010                         // p2 = np21 + (np22 - np21) * out_a2;
1011                         VECSUB(temp, np22, np21);
1012                         mul_v3_fl(temp, *out_a2);
1013                         VECADD(p2, np21, temp);
1014
1015                         calculateEENormal(np11, np12, np21, np22, out_normal);
1016                         VECSUB(temp, p1, p2);
1017                         return INPR(temp, temp);
1018                 }
1019
1020
1021                 /*
1022                 * Clamp both points to the finite edges.
1023                 * The one that moves most during clamping is one part of the solution.
1024                 */
1025                 dist_a1 = *out_a1;
1026                 CLAMP(dist_a1, 0.0, 1.0);
1027                 dist_a2 = *out_a2;
1028                 CLAMP(dist_a2, 0.0, 1.0);
1029
1030                 // Now project the "most clamped" point on the other line.
1031                 if (dist_a1 > dist_a2)
1032                 {
1033                         /* keep out_a1 */
1034                         float p1[3];
1035
1036                         // p1 = np11 + (np12 - np11) * out_a1;
1037                         VECSUB(temp, np12, np11);
1038                         mul_v3_fl(temp, *out_a1);
1039                         VECADD(p1, np11, temp);
1040
1041                         *out_a2 = projectPointOntoLine(p1, np21, np22);
1042                         CLAMP(*out_a2, 0.0, 1.0);
1043
1044                         calculateEENormal(np11, np12, np21, np22, out_normal);
1045
1046                         // return (p1 - (np21 + (np22 - np21) * out_a2)).lengthSquared();
1047                         VECSUB(temp, np22, np21);
1048                         mul_v3_fl(temp, *out_a2);
1049                         VECADD(temp, temp, np21);
1050                         VECSUB(temp, p1, temp);
1051                         return INPR(temp, temp);
1052                 }
1053                 else
1054                 {
1055                         /* keep out_a2 */
1056                         float p2[3];
1057
1058                         // p2 = np21 + (np22 - np21) * out_a2;
1059                         VECSUB(temp, np22, np21);
1060                         mul_v3_fl(temp, *out_a2);
1061                         VECADD(p2, np21, temp);
1062
1063                         *out_a1 = projectPointOntoLine(p2, np11, np12);
1064                         CLAMP(*out_a1, 0.0, 1.0);
1065
1066                         calculateEENormal(np11, np12, np21, np22, out_normal);
1067
1068                         // return ((np11 + (np12 - np11) * out_a1) - p2).lengthSquared();
1069                         VECSUB(temp, np12, np11);
1070                         mul_v3_fl(temp, *out_a1);
1071                         VECADD(temp, temp, np11);
1072                         VECSUB(temp, temp, p2);
1073                         return INPR(temp, temp);
1074                 }
1075         }
1076
1077         printf("Error in edgedge_distance: end of function\n");
1078         return 0;
1079 }
1080
1081 static int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
1082 {
1083         EdgeCollPair edgecollpair;
1084         Cloth *cloth1=NULL;
1085         ClothVertex *verts1=NULL;
1086         unsigned int i = 0, k = 0;
1087         int numsolutions = 0;
1088         double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
1089         double solution[3], solution2[3];
1090         MVert *verts2 = collmd->current_x; // old x
1091         MVert *velocity2 = collmd->current_v; // velocity
1092         float distance = 0;
1093         float triA[3][3], triB[3][3];
1094         int result = 0;
1095
1096         cloth1 = clmd->clothObject;
1097         verts1 = cloth1->verts;
1098
1099         for(i = 0; i < 9; i++)
1100         {
1101                 // 9 edge - edge possibilities
1102
1103                 if(i == 0) // cloth edge: 1-2; coll edge: 1-2
1104                 {
1105                         edgecollpair.p11 = collpair->ap1;
1106                         edgecollpair.p12 = collpair->ap2;
1107
1108                         edgecollpair.p21 = collpair->bp1;
1109                         edgecollpair.p22 = collpair->bp2;
1110                 }
1111                 else if(i == 1) // cloth edge: 1-2; coll edge: 2-3
1112                 {
1113                         edgecollpair.p11 = collpair->ap1;
1114                         edgecollpair.p12 = collpair->ap2;
1115
1116                         edgecollpair.p21 = collpair->bp2;
1117                         edgecollpair.p22 = collpair->bp3;
1118                 }
1119                 else if(i == 2) // cloth edge: 1-2; coll edge: 1-3
1120                 {
1121                         edgecollpair.p11 = collpair->ap1;
1122                         edgecollpair.p12 = collpair->ap2;
1123
1124                         edgecollpair.p21 = collpair->bp1;
1125                         edgecollpair.p22 = collpair->bp3;
1126                 }
1127                 else if(i == 3) // cloth edge: 2-3; coll edge: 1-2
1128                 {
1129                         edgecollpair.p11 = collpair->ap2;
1130                         edgecollpair.p12 = collpair->ap3;
1131
1132                         edgecollpair.p21 = collpair->bp1;
1133                         edgecollpair.p22 = collpair->bp2;
1134                 }
1135                 else if(i == 4) // cloth edge: 2-3; coll edge: 2-3
1136                 {
1137                         edgecollpair.p11 = collpair->ap2;
1138                         edgecollpair.p12 = collpair->ap3;
1139
1140                         edgecollpair.p21 = collpair->bp2;
1141                         edgecollpair.p22 = collpair->bp3;
1142                 }
1143                 else if(i == 5) // cloth edge: 2-3; coll edge: 1-3
1144                 {
1145                         edgecollpair.p11 = collpair->ap2;
1146                         edgecollpair.p12 = collpair->ap3;
1147
1148                         edgecollpair.p21 = collpair->bp1;
1149                         edgecollpair.p22 = collpair->bp3;
1150                 }
1151                 else if(i ==6) // cloth edge: 1-3; coll edge: 1-2
1152                 {
1153                         edgecollpair.p11 = collpair->ap1;
1154                         edgecollpair.p12 = collpair->ap3;
1155
1156                         edgecollpair.p21 = collpair->bp1;
1157                         edgecollpair.p22 = collpair->bp2;
1158                 }
1159                 else if(i ==7) // cloth edge: 1-3; coll edge: 2-3
1160                 {
1161                         edgecollpair.p11 = collpair->ap1;
1162                         edgecollpair.p12 = collpair->ap3;
1163
1164                         edgecollpair.p21 = collpair->bp2;
1165                         edgecollpair.p22 = collpair->bp3;
1166                 }
1167                 else if(i == 8) // cloth edge: 1-3; coll edge: 1-3
1168                 {
1169                         edgecollpair.p11 = collpair->ap1;
1170                         edgecollpair.p12 = collpair->ap3;
1171
1172                         edgecollpair.p21 = collpair->bp1;
1173                         edgecollpair.p22 = collpair->bp3;
1174                 }
1175                 /*
1176                 if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16))
1177                         printf("Ahier!\n");
1178                 if((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3))
1179                         printf("Ahier!\n");
1180                 */
1181
1182                 // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
1183                 {
1184                         // always put coll points in p21/p22
1185                         VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
1186                         VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
1187
1188                         VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
1189                         VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
1190
1191                         VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold );
1192                         VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv );
1193
1194                         numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
1195
1196                         if((edgecollpair.p11 == 3 && edgecollpair.p12==16)|| (edgecollpair.p11==16 && edgecollpair.p12==3))
1197                         {
1198                                 if(edgecollpair.p21==6 || edgecollpair.p22 == 6)
1199                                 {
1200                                         printf("dist: %f, sol[k]: %lf, sol2[k]: %lf\n", distance, solution[k], solution2[k]);
1201                                         printf("a1: %f, a2: %f, b1: %f, b2: %f\n", x1[0], x2[0], x3[0], v1[0]);
1202                                         printf("b21: %d, b22: %d\n", edgecollpair.p21, edgecollpair.p22);
1203                                 }
1204                         }
1205
1206                         for ( k = 0; k < numsolutions; k++ )
1207                         {
1208                                 // printf("sol %d: %lf\n", k, solution[k]);
1209                                 if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) && ( solution[k] >  ALMOST_ZERO))
1210                                 {
1211                                         float a,b;
1212                                         float out_normal[3];
1213                                         float distance;
1214                                         float impulse = 0;
1215                                         float I_mag;
1216
1217                                         // move verts
1218                                         VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, solution[k]);
1219                                         VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, solution[k]);
1220
1221                                         VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, solution[k]);
1222                                         VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, solution[k]);
1223
1224                                         // TODO: check for collisions
1225                                         distance = edgedge_distance(triA[0], triA[1], triB[0], triB[1], &a, &b, out_normal);
1226
1227                                         if ((distance <= clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO) && (INPR(out_normal, out_normal) > 0))
1228                                         {
1229                                                 float vrel_1_to_2[3], temp[3], temp2[3], out_normalVelocity;
1230                                                 float desiredVn;
1231
1232                                                 VECCOPY(vrel_1_to_2, verts1[edgecollpair.p11].tv);
1233                                                 mul_v3_fl(vrel_1_to_2, 1.0 - a);
1234                                                 VECCOPY(temp, verts1[edgecollpair.p12].tv);
1235                                                 mul_v3_fl(temp, a);
1236
1237                                                 VECADD(vrel_1_to_2, vrel_1_to_2, temp);
1238
1239                                                 VECCOPY(temp, verts1[edgecollpair.p21].tv);
1240                                                 mul_v3_fl(temp, 1.0 - b);
1241                                                 VECCOPY(temp2, verts1[edgecollpair.p22].tv);
1242                                                 mul_v3_fl(temp2, b);
1243                                                 VECADD(temp, temp, temp2);
1244
1245                                                 VECSUB(vrel_1_to_2, vrel_1_to_2, temp);
1246
1247                                                 out_normalVelocity = INPR(vrel_1_to_2, out_normal);
1248 /*
1249                                                 // this correction results in wrong normals sometimes?
1250                                                 if(out_normalVelocity < 0.0)
1251                                                 {
1252                                                         out_normalVelocity*= -1.0;
1253                                                         negate_v3(out_normal);
1254                                                 }
1255 */
1256                                                 /* Inelastic repulsion impulse. */
1257
1258                                                 // Calculate which normal velocity we need.
1259                                                 desiredVn = (out_normalVelocity * (float)solution[k] - (.1 * (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree )) - sqrt(distance)) - ALMOST_ZERO);
1260
1261                                                 // Now calculate what impulse we need to reach that velocity.
1262                                                 I_mag = (out_normalVelocity - desiredVn) / 2.0; // / (1/m1 + 1/m2);
1263
1264                                                 // Finally apply that impulse.
1265                                                 impulse = (2.0 * -I_mag) / (a*a + (1.0-a)*(1.0-a) + b*b + (1.0-b)*(1.0-b));
1266
1267                                                 VECADDMUL ( verts1[edgecollpair.p11].impulse, out_normal, (1.0-a) * impulse );
1268                                                 verts1[edgecollpair.p11].impulse_count++;
1269
1270                                                 VECADDMUL ( verts1[edgecollpair.p12].impulse, out_normal, a * impulse );
1271                                                 verts1[edgecollpair.p12].impulse_count++;
1272
1273                                                 // return true;
1274                                                 result = 1;
1275                                                 break;
1276                                         }
1277                                         else
1278                                         {
1279                                                 // missing from collision.hpp
1280                                         }
1281                                         // mintime = MIN2(mintime, (float)solution[k]);
1282
1283                                         break;
1284                                 }
1285                         }
1286                 }
1287         }
1288         return result;
1289 }
1290
1291 static int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
1292 {
1293         Cloth *cloth1;
1294         cloth1 = clmd->clothObject;
1295
1296         for ( ; collpair != collision_end; collpair++ )
1297         {
1298                 // only handle moving collisions here
1299                 if (!( collpair->flag & COLLISION_IN_FUTURE ))
1300                         continue;
1301
1302                 cloth_collision_moving_edges ( clmd, collmd, collpair);
1303                 // cloth_collision_moving_tris ( clmd, collmd, collpair);
1304         }
1305
1306         return 1;
1307 }
1308 #endif
1309
1310 static void add_collision_object(Object ***objs, int *numobj, int *maxobj, Object *ob, Object *self, int level)
1311 {
1312         CollisionModifierData *cmd= NULL;
1313
1314         if(ob == self)
1315                 return;
1316
1317         /* only get objects with collision modifier */
1318         if(ob->pd && ob->pd->deflect)
1319                 cmd= (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
1320
1321         if(cmd) {
1322                 /* extend array */
1323                 if(*numobj >= *maxobj) {
1324                         *maxobj *= 2;
1325                         *objs= MEM_reallocN(*objs, sizeof(Object*)*(*maxobj));
1326                 }
1327
1328                 (*objs)[*numobj] = ob;
1329                 (*numobj)++;
1330         }
1331
1332         /* objects in dupli groups, one level only for now */
1333         if(ob->dup_group && level == 0) {
1334                 GroupObject *go;
1335                 Group *group= ob->dup_group;
1336
1337                 /* add objects */
1338                 for(go= group->gobject.first; go; go= go->next)
1339                         add_collision_object(objs, numobj, maxobj, go->ob, self, level+1);
1340         }
1341 }
1342
1343 // return all collision objects in scene
1344 // collision object will exclude self
1345 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj)
1346 {
1347         Base *base;
1348         Object **objs;
1349         GroupObject *go;
1350         unsigned int numobj= 0, maxobj= 100;
1351
1352         objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
1353
1354         /* gather all collision objects */
1355         if(group) {
1356                 /* use specified group */
1357                 for(go= group->gobject.first; go; go= go->next)
1358                         add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0);
1359         }
1360         else {
1361                 Scene *sce; /* for SETLOOPER macro */
1362                 /* add objects in same layer in scene */
1363                 for(SETLOOPER(scene, base)) {
1364                         if(base->lay & self->lay)
1365                                 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0);
1366
1367                 }
1368         }
1369
1370         *numcollobj= numobj;
1371
1372         return objs;
1373 }
1374
1375 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
1376 {
1377         CollisionModifierData *cmd= NULL;
1378         ColliderCache *col;
1379
1380         if(ob == self)
1381                 return;
1382
1383         if(ob->pd && ob->pd->deflect)
1384                 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
1385
1386         if(cmd && cmd->bvhtree) {
1387                 if(*objs == NULL)
1388                         *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
1389
1390                 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
1391                 col->ob = ob;
1392                 col->collmd = cmd;
1393                 /* make sure collider is properly set up */
1394                 collision_move_object(cmd, 1.0, 0.0);
1395                 BLI_addtail(*objs, col);
1396         }
1397
1398         /* objects in dupli groups, one level only for now */
1399         if(ob->dup_group && level == 0) {
1400                 GroupObject *go;
1401                 Group *group= ob->dup_group;
1402
1403                 /* add objects */
1404                 for(go= group->gobject.first; go; go= go->next)
1405                         add_collider_cache_object(objs, go->ob, self, level+1);
1406         }
1407 }
1408
1409 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
1410 {
1411         GroupObject *go;
1412         ListBase *objs= NULL;
1413
1414         /* add object in same layer in scene */
1415         if(group) {
1416                 for(go= group->gobject.first; go; go= go->next)
1417                         add_collider_cache_object(&objs, go->ob, self, 0);
1418         }
1419         else {
1420                 Scene *sce; /* for SETLOOPER macro */
1421                 Base *base;
1422
1423                 /* add objects in same layer in scene */
1424                 for(SETLOOPER(scene, base)) {
1425                         if(!self || (base->lay & self->lay))
1426                                 add_collider_cache_object(&objs, base->object, self, 0);
1427
1428                 }
1429         }
1430
1431         return objs;
1432 }
1433
1434 void free_collider_cache(ListBase **colliders)
1435 {
1436         if(*colliders) {
1437                 BLI_freelistN(*colliders);
1438                 MEM_freeN(*colliders);
1439                 *colliders = NULL;
1440         }
1441 }
1442
1443 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap)
1444 {
1445         int i;
1446
1447         *collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * numresult * 4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
1448         *collisions_index = *collisions;
1449
1450         for ( i = 0; i < numresult; i++ )
1451         {
1452                 *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, *collisions_index );
1453         }
1454 }
1455
1456 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
1457 {
1458         Cloth *cloth = clmd->clothObject;
1459         int i=0, j = 0, numfaces = 0, numverts = 0;
1460         ClothVertex *verts = NULL;
1461         int ret = 0;
1462         int result = 0;
1463         float tnull[3] = {0,0,0};
1464
1465         numfaces = clmd->clothObject->numfaces;
1466         numverts = clmd->clothObject->numverts;
1467
1468         verts = cloth->verts;
1469
1470         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1471         result = 1;
1472         for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence
1473         {
1474                 result = 0;
1475
1476                 if ( collmd->bvhtree )
1477                 {
1478                         result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
1479
1480                         // apply impulses in parallel
1481                         if ( result )
1482                         {
1483                                 for ( i = 0; i < numverts; i++ )
1484                                 {
1485                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
1486                                         if ( verts[i].impulse_count )
1487                                         {
1488                                                 VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1489                                                 VECCOPY ( verts[i].impulse, tnull );
1490                                                 verts[i].impulse_count = 0;
1491
1492                                                 ret++;
1493                                         }
1494                                 }
1495                         }
1496                 }
1497         }
1498         return ret;
1499 }
1500
1501 // cloth - object collisions
1502 int cloth_bvh_objcollision (Object *ob, ClothModifierData * clmd, float step, float dt )
1503 {
1504         Cloth *cloth= clmd->clothObject;
1505         BVHTree *cloth_bvh= cloth->bvhtree;
1506         unsigned int i=0, numfaces = 0, numverts = 0, k, l, j;
1507         int rounds = 0; // result counts applied collisions; ic is for debug output;
1508         ClothVertex *verts = NULL;
1509         int ret = 0, ret2 = 0;
1510         Object **collobjs = NULL;
1511         unsigned int numcollobj = 0;
1512
1513         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
1514                 return 0;
1515
1516         verts = cloth->verts;
1517         numfaces = cloth->numfaces;
1518         numverts = cloth->numverts;
1519
1520         ////////////////////////////////////////////////////////////
1521         // static collisions
1522         ////////////////////////////////////////////////////////////
1523
1524         // update cloth bvh
1525         bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1526         bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1527
1528         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj);
1529
1530         if(!collobjs)
1531                 return 0;
1532
1533         do
1534         {
1535                 CollPair **collisions, **collisions_index;
1536
1537                 ret2 = 0;
1538
1539                 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
1540                 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
1541
1542                 // check all collision objects
1543                 for(i = 0; i < numcollobj; i++)
1544                 {
1545                         Object *collob= collobjs[i];
1546                         CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision);
1547                         BVHTreeOverlap *overlap = NULL;
1548                         int result = 0;
1549
1550                         if(!collmd->bvhtree)
1551                                 continue;
1552
1553                         /* move object to position (step) in time */
1554                         collision_move_object ( collmd, step + dt, step );
1555
1556                         /* search for overlapping collision pairs */
1557                         overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
1558
1559                         // go to next object if no overlap is there
1560                         if( result && overlap ) {
1561                                 /* check if collisions really happen (costly near check) */
1562                                 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], &collisions_index[i], result, overlap);
1563
1564                                 // resolve nearby collisions
1565                                 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
1566                                 ret2 += ret;
1567                         }
1568
1569                         if ( overlap )
1570                                 MEM_freeN ( overlap );
1571                 }
1572                 rounds++;
1573
1574                 for(i = 0; i < numcollobj; i++)
1575                 {
1576                         if ( collisions[i] ) MEM_freeN ( collisions[i] );
1577                 }
1578
1579                 MEM_freeN(collisions);
1580                 MEM_freeN(collisions_index);
1581
1582                 ////////////////////////////////////////////////////////////
1583                 // update positions
1584                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1585                 ////////////////////////////////////////////////////////////
1586
1587                 // verts come from clmd
1588                 for ( i = 0; i < numverts; i++ )
1589                 {
1590                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1591                         {
1592                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1593                                 {
1594                                         continue;
1595                                 }
1596                         }
1597
1598                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
1599                 }
1600                 ////////////////////////////////////////////////////////////
1601
1602
1603                 ////////////////////////////////////////////////////////////
1604                 // Test on *simple* selfcollisions
1605                 ////////////////////////////////////////////////////////////
1606                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
1607                 {
1608                         for(l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++)
1609                         {
1610                                 // TODO: add coll quality rounds again
1611                                 BVHTreeOverlap *overlap = NULL;
1612                                 unsigned int result = 0;
1613
1614                                 // collisions = 1;
1615                                 verts = cloth->verts; // needed for openMP
1616
1617                                 numfaces = cloth->numfaces;
1618                                 numverts = cloth->numverts;
1619
1620                                 verts = cloth->verts;
1621
1622                                 if ( cloth->bvhselftree )
1623                                 {
1624                                         // search for overlapping collision pairs
1625                                         overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
1626
1627         // #pragma omp parallel for private(k, i, j) schedule(static)
1628                                         for ( k = 0; k < result; k++ )
1629                                         {
1630                                                 float temp[3];
1631                                                 float length = 0;
1632                                                 float mindistance;
1633
1634                                                 i = overlap[k].indexA;
1635                                                 j = overlap[k].indexB;
1636
1637                                                 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
1638
1639                                                 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1640                                                 {
1641                                                         if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1642                                                                                 && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
1643                                                         {
1644                                                                 continue;
1645                                                         }
1646                                                 }
1647
1648                                                 VECSUB ( temp, verts[i].tx, verts[j].tx );
1649
1650                                                 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
1651
1652                                                 // check for adjacent points (i must be smaller j)
1653                                                 if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
1654                                                 {
1655                                                         continue;
1656                                                 }
1657
1658                                                 length = normalize_v3( temp );
1659
1660                                                 if ( length < mindistance )
1661                                                 {
1662                                                         float correction = mindistance - length;
1663
1664                                                         if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1665                                                         {
1666                                                                 mul_v3_fl( temp, -correction );
1667                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
1668                                                         }
1669                                                         else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
1670                                                         {
1671                                                                 mul_v3_fl( temp, correction );
1672                                                                 VECADD ( verts[i].tx, verts[i].tx, temp );
1673                                                         }
1674                                                         else
1675                                                         {
1676                                                                 mul_v3_fl( temp, -correction*0.5 );
1677                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
1678
1679                                                                 VECSUB ( verts[i].tx, verts[i].tx, temp );
1680                                                         }
1681                                                         ret = 1;
1682                                                         ret2 += ret;
1683                                                 }
1684                                                 else
1685                                                 {
1686                                                         // check for approximated time collisions
1687                                                 }
1688                                         }
1689
1690                                         if ( overlap )
1691                                                 MEM_freeN ( overlap );
1692
1693                                 }
1694                         }
1695                         ////////////////////////////////////////////////////////////
1696
1697                         ////////////////////////////////////////////////////////////
1698                         // SELFCOLLISIONS: update velocities
1699                         ////////////////////////////////////////////////////////////
1700                         if ( ret2 )
1701                         {
1702                                 for ( i = 0; i < cloth->numverts; i++ )
1703                                 {
1704                                         if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) )
1705                                         {
1706                                                 VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold );
1707                                         }
1708                                 }
1709                         }
1710                         ////////////////////////////////////////////////////////////
1711                 }
1712         }
1713         while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
1714
1715         if(collobjs)
1716                 MEM_freeN(collobjs);
1717
1718         return MIN2 ( ret, 1 );
1719 }