source/blender/blenkernel/intern/collision.c

   1 /*
   2  * ***** BEGIN GPL LICENSE BLOCK *****
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software Foundation,
  16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  17  *
  18  * The Original Code is Copyright (C) Blender Foundation
  19  * All rights reserved.
  20  *
  21  * The Original Code is: all of this file.
  22  *
  23  * Contributor(s): none yet.
  24  *
  25  * ***** END GPL LICENSE BLOCK *****
  26  */
  27
  28 /** \file blender/blenkernel/intern/collision.c
  29  *  \ingroup bke
  30  */
  31
  32
  33 #include "MEM_guardedalloc.h"
  34
  35 #include "BKE_cloth.h"
  36
  37 #include "DNA_cloth_types.h"
  38 #include "DNA_group_types.h"
  39 #include "DNA_mesh_types.h"
  40 #include "DNA_object_types.h"
  41 #include "DNA_object_force.h"
  42 #include "DNA_scene_types.h"
  43 #include "DNA_meshdata_types.h"
  44
  45 #include "BLI_utildefines.h"
  46 #include "BLI_blenlib.h"
  47 #include "BLI_math.h"
  48 #include "BLI_edgehash.h"
  49 #include "BLI_utildefines.h"
  50 #include "BLI_ghash.h"
  51 #include "BLI_memarena.h"
  52 #include "BLI_rand.h"
  53
  54 #include "BKE_DerivedMesh.h"
  55 #include "BKE_global.h"
  56 #include "BKE_scene.h"
  57 #include "BKE_mesh.h"
  58 #include "BKE_object.h"
  59 #include "BKE_modifier.h"
  60
  61 #include "BKE_DerivedMesh.h"
  62 #ifdef USE_BULLET
  63 #include "Bullet-C-Api.h"
  64 #endif
  65 #include "BLI_kdopbvh.h"
  66 #include "BKE_collision.h"
  67
  68 #ifdef WITH_ELTOPO
  69 #include "eltopo-capi.h"
  70 #endif
  71
  72
  73 /***********************************
  74 Collision modifier code start
  75 ***********************************/
  76
  77 /* step is limited from 0 (frame start position) to 1 (frame end position) */
  78 void collision_move_object(CollisionModifierData *collmd, float step, float prevstep)
  79 {
  80         float tv[3] = {0, 0, 0};
  81         unsigned int i = 0;
  82
  83         for ( i = 0; i < collmd->numverts; i++ ) {
  84                 sub_v3_v3v3(tv, collmd->xnew[i].co, collmd->x[i].co);
  85                 VECADDS(collmd->current_x[i].co, collmd->x[i].co, tv, prevstep);
  86                 VECADDS(collmd->current_xnew[i].co, collmd->x[i].co, tv, step);
  87                 sub_v3_v3v3(collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co);
  88         }
  89
  90         bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
  91 }
  92
  93 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
  94 {
  95         BVHTree *tree;
  96         float co[12];
  97         unsigned int i;
  98         MFace *tface = mfaces;
  99
 100         tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
 101
 102         // fill tree
 103         for ( i = 0; i < numfaces; i++, tface++ ) {
 104                 copy_v3_v3 ( &co[0*3], x[tface->v1].co );
 105                 copy_v3_v3 ( &co[1*3], x[tface->v2].co );
 106                 copy_v3_v3 ( &co[2*3], x[tface->v3].co );
 107                 if ( tface->v4 )
 108                         copy_v3_v3 ( &co[3*3], x[tface->v4].co );
 109
 110                 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
 111         }
 112
 113         // balance tree
 114         BLI_bvhtree_balance ( tree );
 115
 116         return tree;
 117 }
 118
 119 void bvhtree_update_from_mvert(BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
 120 {
 121         int i;
 122         MFace *mfaces = faces;
 123         float co[12], co_moving[12];
 124         int ret = 0;
 125
 126         if ( !bvhtree )
 127                 return;
 128
 129         if ( x ) {
 130                 for ( i = 0; i < numfaces; i++, mfaces++ ) {
 131                         copy_v3_v3 ( &co[0*3], x[mfaces->v1].co );
 132                         copy_v3_v3 ( &co[1*3], x[mfaces->v2].co );
 133                         copy_v3_v3 ( &co[2*3], x[mfaces->v3].co );
 134                         if ( mfaces->v4 )
 135                                 copy_v3_v3 ( &co[3*3], x[mfaces->v4].co );
 136
 137                         // copy new locations into array
 138                         if ( moving && xnew ) {
 139                                 // update moving positions
 140                                 copy_v3_v3 ( &co_moving[0*3], xnew[mfaces->v1].co );
 141                                 copy_v3_v3 ( &co_moving[1*3], xnew[mfaces->v2].co );
 142                                 copy_v3_v3 ( &co_moving[2*3], xnew[mfaces->v3].co );
 143                                 if ( mfaces->v4 )
 144                                         copy_v3_v3 ( &co_moving[3*3], xnew[mfaces->v4].co );
 145
 146                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
 147                         }
 148                         else {
 149                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
 150                         }
 151
 152                         // check if tree is already full
 153                         if ( !ret )
 154                                 break;
 155                 }
 156
 157                 BLI_bvhtree_update_tree ( bvhtree );
 158         }
 159 }
 160
 161 /***********************************
 162 Collision modifier code end
 163 ***********************************/
 164
 165 // w3 is not perfect
 166 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
 167 {
 168         double  tempV1[3], tempV2[3], tempV4[3];
 169         double  a, b, c, d, e, f;
 170
 171         VECSUB ( tempV1, p1, p3 );
 172         VECSUB ( tempV2, p2, p3 );
 173         VECSUB ( tempV4, pv, p3 );
 174
 175         a = INPR ( tempV1, tempV1 );
 176         b = INPR ( tempV1, tempV2 );
 177         c = INPR ( tempV2, tempV2 );
 178         e = INPR ( tempV1, tempV4 );
 179         f = INPR ( tempV2, tempV4 );
 180
 181         d = ( a * c - b * b );
 182
 183         if ( ABS ( d ) < (double)ALMOST_ZERO ) {
 184                 *w1 = *w2 = *w3 = 1.0 / 3.0;
 185                 return;
 186         }
 187
 188         w1[0] = ( float ) ( ( e * c - b * f ) / d );
 189
 190         if ( w1[0] < 0 )
 191                 w1[0] = 0;
 192
 193         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
 194
 195         if ( w2[0] < 0 )
 196                 w2[0] = 0;
 197
 198         w3[0] = 1.0f - w1[0] - w2[0];
 199 }
 200
 201 #pragma GCC diagnostic push
 202 #pragma GCC diagnostic ignored "-Wdouble-promotion"
 203
 204 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
 205 {
 206         zero_v3(to);
 207         VECADDMUL(to, v1, w1);
 208         VECADDMUL(to, v2, w2);
 209         VECADDMUL(to, v3, w3);
 210 }
 211
 212 static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
 213 {
 214         int result = 0;
 215         Cloth *cloth1;
 216         float w1, w2, w3, u1, u2, u3;
 217         float v1[3], v2[3], relativeVelocity[3];
 218         float magrelVel;
 219         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
 220
 221         cloth1 = clmd->clothObject;
 222
 223         for ( ; collpair != collision_end; collpair++ ) {
 224                 float i1[3], i2[3], i3[3];
 225
 226                 zero_v3(i1);
 227                 zero_v3(i2);
 228                 zero_v3(i3);
 229
 230                 /* only handle static collisions here */
 231                 if ( collpair->flag & COLLISION_IN_FUTURE )
 232                         continue;
 233
 234                 /* compute barycentric coordinates for both collision points */
 235                 collision_compute_barycentric ( collpair->pa,
 236                         cloth1->verts[collpair->ap1].txold,
 237                         cloth1->verts[collpair->ap2].txold,
 238                         cloth1->verts[collpair->ap3].txold,
 239                         &w1, &w2, &w3 );
 240
 241                 /* was: txold */
 242                 collision_compute_barycentric ( collpair->pb,
 243                         collmd->current_x[collpair->bp1].co,
 244                         collmd->current_x[collpair->bp2].co,
 245                         collmd->current_x[collpair->bp3].co,
 246                         &u1, &u2, &u3 );
 247
 248                 /* Calculate relative "velocity". */
 249                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
 250
 251                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
 252
 253                 sub_v3_v3v3(relativeVelocity, v2, v1);
 254
 255                 /* Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). */
 256                 magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
 257
 258                 /* printf("magrelVel: %f\n", magrelVel); */
 259
 260                 /* Calculate masses of points.
 261                  * TODO */
 262
 263                 /* If v_n_mag < 0 the edges are approaching each other. */
 264                 if ( magrelVel > ALMOST_ZERO ) {
 265                         /* Calculate Impulse magnitude to stop all motion in normal direction. */
 266                         float magtangent = 0, repulse = 0, d = 0;
 267                         double impulse = 0.0;
 268                         float vrel_t_pre[3];
 269                         float temp[3], spf;
 270
 271                         /* calculate tangential velocity */
 272                         copy_v3_v3 ( temp, collpair->normal );
 273                         mul_v3_fl(temp, magrelVel);
 274                         sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
 275
 276                         /* Decrease in magnitude of relative tangential velocity due to coulomb friction
 277                          * in original formula "magrelVel" should be the "change of relative velocity in normal direction" */
 278                         magtangent = min_ff(clmd->coll_parms->friction * 0.01f * magrelVel, sqrtf(dot_v3v3(vrel_t_pre, vrel_t_pre)));
 279
 280                         /* Apply friction impulse. */
 281                         if ( magtangent > ALMOST_ZERO ) {
 282                                 normalize_v3(vrel_t_pre);
 283
 284                                 impulse = magtangent / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* 2.0 * */
 285                                 VECADDMUL ( i1, vrel_t_pre, w1 * impulse );
 286                                 VECADDMUL ( i2, vrel_t_pre, w2 * impulse );
 287                                 VECADDMUL ( i3, vrel_t_pre, w3 * impulse );
 288                         }
 289
 290                         /* Apply velocity stopping impulse
 291                          * I_c = m * v_N / 2.0
 292                          * no 2.0 * magrelVel normally, but looks nicer DG */
 293                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
 294
 295                         VECADDMUL ( i1, collpair->normal, w1 * impulse );
 296                         cloth1->verts[collpair->ap1].impulse_count++;
 297
 298                         VECADDMUL ( i2, collpair->normal, w2 * impulse );
 299                         cloth1->verts[collpair->ap2].impulse_count++;
 300
 301                         VECADDMUL ( i3, collpair->normal, w3 * impulse );
 302                         cloth1->verts[collpair->ap3].impulse_count++;
 303
 304                         /* Apply repulse impulse if distance too short
 305                          * I_r = -min(dt*kd, m(0, 1d/dt - v_n))
 306                          * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this:
 307                          * v += impulse; x_new = x + v;
 308                          * We don't use dt!!
 309                          * DG TODO: Fix usage of dt here! */
 310                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
 311
 312                         d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
 313                         if ( ( magrelVel < 0.1f*d*spf ) && ( d > ALMOST_ZERO ) ) {
 314                                 repulse = MIN2 ( d*1.0f/spf, 0.1f*d*spf - magrelVel );
 315
 316                                 /* stay on the safe side and clamp repulse */
 317                                 if ( impulse > ALMOST_ZERO )
 318                                         repulse = min_ff( repulse, 5.0*impulse );
 319                                 repulse = max_ff(impulse, repulse);
 320
 321                                 impulse = repulse / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* original 2.0 / 0.25 */
 322                                 VECADDMUL ( i1, collpair->normal,  impulse );
 323                                 VECADDMUL ( i2, collpair->normal,  impulse );
 324                                 VECADDMUL ( i3, collpair->normal,  impulse );
 325                         }
 326
 327                         result = 1;
 328                 }
 329                 else {
 330                         /* Apply repulse impulse if distance too short
 331                          * I_r = -min(dt*kd, max(0, 1d/dt - v_n))
 332                          * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this:
 333                          * v += impulse; x_new = x + v;
 334                          * We don't use dt!! */
 335                         float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
 336
 337                         float d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - (float)collpair->distance;
 338                         if ( d > ALMOST_ZERO) {
 339                                 /* stay on the safe side and clamp repulse */
 340                                 float repulse = d*1.0f/spf;
 341
 342                                 float impulse = repulse / ( 3.0f * ( 1.0f + w1*w1 + w2*w2 + w3*w3 )); /* original 2.0 / 0.25 */
 343
 344                                 VECADDMUL ( i1, collpair->normal,  impulse );
 345                                 VECADDMUL ( i2, collpair->normal,  impulse );
 346                                 VECADDMUL ( i3, collpair->normal,  impulse );
 347
 348                                 cloth1->verts[collpair->ap1].impulse_count++;
 349                                 cloth1->verts[collpair->ap2].impulse_count++;
 350                                 cloth1->verts[collpair->ap3].impulse_count++;
 351
 352                                 result = 1;
 353                         }
 354                 }
 355
 356                 if (result) {
 357                         int i = 0;
 358
 359                         for (i = 0; i < 3; i++) {
 360                                 if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[i]) < ABS(i1[i]))
 361                                         cloth1->verts[collpair->ap1].impulse[i] = i1[i];
 362
 363                                 if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[i]) < ABS(i2[i]))
 364                                         cloth1->verts[collpair->ap2].impulse[i] = i2[i];
 365
 366                                 if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[i]) < ABS(i3[i]))
 367                                         cloth1->verts[collpair->ap3].impulse[i] = i3[i];
 368                         }
 369                 }
 370         }
 371         return result;
 372 }
 373
 374 #pragma GCC diagnostic pop
 375
 376 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
 377 static CollPair* cloth_collision(ModifierData *md1, ModifierData *md2,
 378                                  BVHTreeOverlap *overlap, CollPair *collpair, float UNUSED(dt))
 379 {
 380         ClothModifierData *clmd = (ClothModifierData *)md1;
 381         CollisionModifierData *collmd = (CollisionModifierData *) md2;
 382         /* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
 383         MFace *face1=NULL, *face2 = NULL;
 384 #ifdef USE_BULLET
 385         ClothVertex *verts1 = clmd->clothObject->verts;
 386 #endif
 387         double distance = 0;
 388         float epsilon1 = clmd->coll_parms->epsilon;
 389         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
 390         int i;
 391
 392         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
 393         face2 = & ( collmd->mfaces[overlap->indexB] );
 394
 395         // check all 4 possible collisions
 396         for ( i = 0; i < 4; i++ ) {
 397                 if ( i == 0 ) {
 398                         // fill faceA
 399                         collpair->ap1 = face1->v1;
 400                         collpair->ap2 = face1->v2;
 401                         collpair->ap3 = face1->v3;
 402
 403                         // fill faceB
 404                         collpair->bp1 = face2->v1;
 405                         collpair->bp2 = face2->v2;
 406                         collpair->bp3 = face2->v3;
 407                 }
 408                 else if ( i == 1 ) {
 409                         if ( face1->v4 ) {
 410                                 // fill faceA
 411                                 collpair->ap1 = face1->v1;
 412                                 collpair->ap2 = face1->v3;
 413                                 collpair->ap3 = face1->v4;
 414
 415                                 // fill faceB
 416                                 collpair->bp1 = face2->v1;
 417                                 collpair->bp2 = face2->v2;
 418                                 collpair->bp3 = face2->v3;
 419                         }
 420                         else {
 421                                 i++;
 422                         }
 423                 }
 424                 if ( i == 2 ) {
 425                         if ( face2->v4 ) {
 426                                 // fill faceA
 427                                 collpair->ap1 = face1->v1;
 428                                 collpair->ap2 = face1->v2;
 429                                 collpair->ap3 = face1->v3;
 430
 431                                 // fill faceB
 432                                 collpair->bp1 = face2->v1;
 433                                 collpair->bp2 = face2->v4;
 434                                 collpair->bp3 = face2->v3;
 435                         }
 436                         else {
 437                                 break;
 438                         }
 439                 }
 440                 else if ( i == 3 ) {
 441                         if ( face1->v4 && face2->v4 ) {
 442                                 // fill faceA
 443                                 collpair->ap1 = face1->v1;
 444                                 collpair->ap2 = face1->v3;
 445                                 collpair->ap3 = face1->v4;
 446
 447                                 // fill faceB
 448                                 collpair->bp1 = face2->v1;
 449                                 collpair->bp2 = face2->v3;
 450                                 collpair->bp3 = face2->v4;
 451                         }
 452                         else {
 453                                 break;
 454                         }
 455                 }
 456
 457 #ifdef USE_BULLET
 458                 // calc distance + normal
 459                 distance = plNearestPoints (
 460                         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 );
 461 #else
 462                 // just be sure that we don't add anything
 463                 distance = 2.0 * (double)( epsilon1 + epsilon2 + ALMOST_ZERO );
 464 #endif
 465
 466                 // distance -1 means no collision result
 467                 if (distance != -1.0 && (distance <= (double)(epsilon1 + epsilon2 + ALMOST_ZERO))) {
 468                         normalize_v3_v3(collpair->normal, collpair->vector);
 469
 470                         collpair->distance = distance;
 471                         collpair->flag = 0;
 472                         collpair++;
 473                 }/*
 474                 else {
 475                         float w1, w2, w3, u1, u2, u3;
 476                         float v1[3], v2[3], relativeVelocity[3];
 477
 478                         // calc relative velocity
 479
 480                         // compute barycentric coordinates for both collision points
 481                         collision_compute_barycentric ( collpair->pa,
 482                         verts1[collpair->ap1].txold,
 483                         verts1[collpair->ap2].txold,
 484                         verts1[collpair->ap3].txold,
 485                         &w1, &w2, &w3 );
 486
 487                         // was: txold
 488                         collision_compute_barycentric ( collpair->pb,
 489                         collmd->current_x[collpair->bp1].co,
 490                         collmd->current_x[collpair->bp2].co,
 491                         collmd->current_x[collpair->bp3].co,
 492                         &u1, &u2, &u3 );
 493
 494                         // Calculate relative "velocity".
 495                         collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
 496
 497                         collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
 498
 499                         sub_v3_v3v3(relativeVelocity, v2, v1);
 500
 501                         if (sqrt(dot_v3v3(relativeVelocity, relativeVelocity)) >= distance)
 502                         {
 503                                 // check for collision in the future
 504                                 collpair->flag |= COLLISION_IN_FUTURE;
 505                                 collpair++;
 506                         }
 507                 }*/
 508         }
 509         return collpair;
 510 }
 511
 512 static void add_collision_object(Object ***objs, unsigned int *numobj, unsigned int *maxobj, Object *ob, Object *self, int level, unsigned int modifier_type)
 513 {
 514         CollisionModifierData *cmd= NULL;
 515
 516         if (ob == self)
 517                 return;
 518
 519         /* only get objects with collision modifier */
 520         if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) || (modifier_type != eModifierType_Collision))
 521                 cmd= (CollisionModifierData *)modifiers_findByType(ob, modifier_type);
 522
 523         if (cmd) {
 524                 /* extend array */
 525                 if (*numobj >= *maxobj) {
 526                         *maxobj *= 2;
 527                         *objs= MEM_reallocN(*objs, sizeof(Object*)*(*maxobj));
 528                 }
 529
 530                 (*objs)[*numobj] = ob;
 531                 (*numobj)++;
 532         }
 533
 534         /* objects in dupli groups, one level only for now */
 535         if (ob->dup_group && level == 0) {
 536                 GroupObject *go;
 537                 Group *group= ob->dup_group;
 538
 539                 /* add objects */
 540                 for (go= group->gobject.first; go; go= go->next)
 541                         add_collision_object(objs, numobj, maxobj, go->ob, self, level+1, modifier_type);
 542         }
 543 }
 544
 545 // return all collision objects in scene
 546 // collision object will exclude self
 547 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj, unsigned int modifier_type)
 548 {
 549         Base *base;
 550         Object **objs;
 551         GroupObject *go;
 552         unsigned int numobj= 0, maxobj= 100;
 553
 554         objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
 555
 556         /* gather all collision objects */
 557         if (group) {
 558                 /* use specified group */
 559                 for (go= group->gobject.first; go; go= go->next)
 560                         add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0, modifier_type);
 561         }
 562         else {
 563                 Scene *sce_iter;
 564                 /* add objects in same layer in scene */
 565                 for (SETLOOPER(scene, sce_iter, base)) {
 566                         if (base->lay & self->lay)
 567                                 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0, modifier_type);
 568
 569                 }
 570         }
 571
 572         *numcollobj= numobj;
 573
 574         return objs;
 575 }
 576
 577 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
 578 {
 579         CollisionModifierData *cmd= NULL;
 580         ColliderCache *col;
 581
 582         if (ob == self)
 583                 return;
 584
 585         if (ob->pd && ob->pd->deflect)
 586                 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
 587
 588         if (cmd && cmd->bvhtree) {
 589                 if (*objs == NULL)
 590                         *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
 591
 592                 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
 593                 col->ob = ob;
 594                 col->collmd = cmd;
 595                 /* make sure collider is properly set up */
 596                 collision_move_object(cmd, 1.0, 0.0);
 597                 BLI_addtail(*objs, col);
 598         }
 599
 600         /* objects in dupli groups, one level only for now */
 601         if (ob->dup_group && level == 0) {
 602                 GroupObject *go;
 603                 Group *group= ob->dup_group;
 604
 605                 /* add objects */
 606                 for (go= group->gobject.first; go; go= go->next)
 607                         add_collider_cache_object(objs, go->ob, self, level+1);
 608         }
 609 }
 610
 611 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
 612 {
 613         GroupObject *go;
 614         ListBase *objs= NULL;
 615
 616         /* add object in same layer in scene */
 617         if (group) {
 618                 for (go= group->gobject.first; go; go= go->next)
 619                         add_collider_cache_object(&objs, go->ob, self, 0);
 620         }
 621         else {
 622                 Scene *sce_iter;
 623                 Base *base;
 624
 625                 /* add objects in same layer in scene */
 626                 for (SETLOOPER(scene, sce_iter, base)) {
 627                         if (!self || (base->lay & self->lay))
 628                                 add_collider_cache_object(&objs, base->object, self, 0);
 629
 630                 }
 631         }
 632
 633         return objs;
 634 }
 635
 636 void free_collider_cache(ListBase **colliders)
 637 {
 638         if (*colliders) {
 639                 BLI_freelistN(*colliders);
 640                 MEM_freeN(*colliders);
 641                 *colliders = NULL;
 642         }
 643 }
 644
 645
 646 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd,
 647         CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap, double dt)
 648 {
 649         int i;
 650
 651         *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 64, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
 652         *collisions_index = *collisions;
 653
 654         for ( i = 0; i < numresult; i++ ) {
 655                 *collisions_index = cloth_collision ( (ModifierData *)clmd, (ModifierData *)collmd,
 656                                                       overlap+i, *collisions_index, dt );
 657         }
 658 }
 659
 660 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
 661 {
 662         Cloth *cloth = clmd->clothObject;
 663         int i=0, j = 0, /*numfaces = 0, */ numverts = 0;
 664         ClothVertex *verts = NULL;
 665         int ret = 0;
 666         int result = 0;
 667
 668         numverts = clmd->clothObject->numverts;
 669         verts = cloth->verts;
 670
 671         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
 672         result = 1;
 673         for ( j = 0; j < 2; j++ ) { /* 5 is just a value that ensures convergence */
 674                 result = 0;
 675
 676                 if ( collmd->bvhtree ) {
 677                         result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
 678
 679                         // apply impulses in parallel
 680                         if (result) {
 681                                 for (i = 0; i < numverts; i++) {
 682                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
 683                                         if (verts[i].impulse_count) {
 684                                                 // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
 685                                                 VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
 686                                                 zero_v3(verts[i].impulse);
 687                                                 verts[i].impulse_count = 0;
 688
 689                                                 ret++;
 690                                         }
 691                                 }
 692                         }
 693                 }
 694
 695                 if (!result) {
 696                         break;
 697                 }
 698         }
 699         return ret;
 700 }
 701
 702 // cloth - object collisions
 703 int cloth_bvh_objcollision(Object *ob, ClothModifierData * clmd, float step, float dt )
 704 {
 705         Cloth *cloth= clmd->clothObject;
 706         BVHTree *cloth_bvh= cloth->bvhtree;
 707         unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ numverts = 0, k, l, j;
 708         int rounds = 0; // result counts applied collisions; ic is for debug output;
 709         ClothVertex *verts = NULL;
 710         int ret = 0, ret2 = 0;
 711         Object **collobjs = NULL;
 712         unsigned int numcollobj = 0;
 713
 714         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
 715                 return 0;
 716
 717         verts = cloth->verts;
 718         /* numfaces = cloth->numfaces; */ /* UNUSED */
 719         numverts = cloth->numverts;
 720
 721         ////////////////////////////////////////////////////////////
 722         // static collisions
 723         ////////////////////////////////////////////////////////////
 724
 725         // update cloth bvh
 726         bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
 727         bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
 728
 729         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
 730
 731         if (!collobjs)
 732                 return 0;
 733
 734         /* move object to position (step) in time */
 735         for (i = 0; i < numcollobj; i++) {
 736                 Object *collob= collobjs[i];
 737                 CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision);
 738
 739                 if (!collmd->bvhtree)
 740                         continue;
 741
 742                 /* move object to position (step) in time */
 743                 collision_move_object ( collmd, step + dt, step );
 744         }
 745
 746         do {
 747                 CollPair **collisions, **collisions_index;
 748
 749                 ret2 = 0;
 750
 751                 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
 752                 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
 753
 754                 // check all collision objects
 755                 for (i = 0; i < numcollobj; i++) {
 756                         Object *collob= collobjs[i];
 757                         CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision);
 758                         BVHTreeOverlap *overlap = NULL;
 759                         unsigned int result = 0;
 760
 761                         if (!collmd->bvhtree)
 762                                 continue;
 763
 764                         /* search for overlapping collision pairs */
 765                         overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
 766
 767                         // go to next object if no overlap is there
 768                         if ( result && overlap ) {
 769                                 /* check if collisions really happen (costly near check) */
 770                                 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i],
 771                                         &collisions_index[i], result, overlap, dt/(float)clmd->coll_parms->loop_count);
 772
 773                                 // resolve nearby collisions
 774                                 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
 775                                 ret2 += ret;
 776                         }
 777
 778                         if ( overlap )
 779                                 MEM_freeN ( overlap );
 780                 }
 781                 rounds++;
 782
 783                 for (i = 0; i < numcollobj; i++) {
 784                         if ( collisions[i] ) MEM_freeN ( collisions[i] );
 785                 }
 786
 787                 MEM_freeN(collisions);
 788                 MEM_freeN(collisions_index);
 789
 790                 ////////////////////////////////////////////////////////////
 791                 // update positions
 792                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
 793                 ////////////////////////////////////////////////////////////
 794
 795                 // verts come from clmd
 796                 for ( i = 0; i < numverts; i++ ) {
 797                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
 798                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
 799                                         continue;
 800                                 }
 801                         }
 802
 803                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
 804                 }
 805                 ////////////////////////////////////////////////////////////
 806
 807
 808                 ////////////////////////////////////////////////////////////
 809                 // Test on *simple* selfcollisions
 810                 ////////////////////////////////////////////////////////////
 811                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF ) {
 812                         for (l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++) {
 813                                 /* TODO: add coll quality rounds again */
 814                                 BVHTreeOverlap *overlap = NULL;
 815                                 unsigned int result = 0;
 816
 817                                 // collisions = 1;
 818                                 verts = cloth->verts; // needed for openMP
 819
 820                                 /* numfaces = cloth->numfaces; */ /* UNUSED */
 821                                 numverts = cloth->numverts;
 822
 823                                 verts = cloth->verts;
 824
 825                                 if ( cloth->bvhselftree ) {
 826                                         // search for overlapping collision pairs
 827                                         overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
 828
 829         // #pragma omp parallel for private(k, i, j) schedule(static)
 830                                         for ( k = 0; k < result; k++ ) {
 831                                                 float temp[3];
 832                                                 float length = 0;
 833                                                 float mindistance;
 834
 835                                                 i = overlap[k].indexA;
 836                                                 j = overlap[k].indexB;
 837
 838                                                 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
 839
 840                                                 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
 841                                                         if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) &&
 842                                                              ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
 843                                                         {
 844                                                                 continue;
 845                                                         }
 846                                                 }
 847
 848                                                 if ((cloth->verts[i].flags & CLOTH_VERT_FLAG_NOSELFCOLL) ||
 849                                                     (cloth->verts[j].flags & CLOTH_VERT_FLAG_NOSELFCOLL))
 850                                                 {
 851                                                         continue;
 852                                                 }
 853
 854                                                 sub_v3_v3v3(temp, verts[i].tx, verts[j].tx);
 855
 856                                                 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
 857
 858                                                 // check for adjacent points (i must be smaller j)
 859                                                 if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) ) {
 860                                                         continue;
 861                                                 }
 862
 863                                                 length = normalize_v3(temp );
 864
 865                                                 if ( length < mindistance ) {
 866                                                         float correction = mindistance - length;
 867
 868                                                         if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
 869                                                                 mul_v3_fl(temp, -correction);
 870                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
 871                                                         }
 872                                                         else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) {
 873                                                                 mul_v3_fl(temp, correction);
 874                                                                 VECADD ( verts[i].tx, verts[i].tx, temp );
 875                                                         }
 876                                                         else {
 877                                                                 mul_v3_fl(temp, correction * -0.5f);
 878                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
 879
 880                                                                 sub_v3_v3v3(verts[i].tx, verts[i].tx, temp);
 881                                                         }
 882                                                         ret = 1;
 883                                                         ret2 += ret;
 884                                                 }
 885                                                 else {
 886                                                         // check for approximated time collisions
 887                                                 }
 888                                         }
 889
 890                                         if ( overlap )
 891                                                 MEM_freeN ( overlap );
 892
 893                                 }
 894                         }
 895                         ////////////////////////////////////////////////////////////
 896
 897                         ////////////////////////////////////////////////////////////
 898                         // SELFCOLLISIONS: update velocities
 899                         ////////////////////////////////////////////////////////////
 900                         if ( ret2 ) {
 901                                 for ( i = 0; i < cloth->numverts; i++ ) {
 902                                         if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) {
 903                                                 sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
 904                                         }
 905                                 }
 906                         }
 907                         ////////////////////////////////////////////////////////////
 908                 }
 909         }
 910         while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
 911
 912         if (collobjs)
 913                 MEM_freeN(collobjs);
 914
 915         return 1|MIN2 ( ret, 1 );
 916 }