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 "DNA_cloth_types.h"
  36 #include "DNA_collection_types.h"
  37 #include "DNA_effect_types.h"
  38 #include "DNA_object_types.h"
  39 #include "DNA_object_force_types.h"
  40 #include "DNA_scene_types.h"
  41 #include "DNA_meshdata_types.h"
  42
  43 #include "BLI_utildefines.h"
  44 #include "BLI_blenlib.h"
  45 #include "BLI_math.h"
  46 #include "BLI_task.h"
  47 #include "BLI_threads.h"
  48
  49 #include "BKE_cloth.h"
  50 #include "BKE_collection.h"
  51 #include "BKE_effect.h"
  52 #include "BKE_layer.h"
  53 #include "BKE_modifier.h"
  54 #include "BKE_scene.h"
  55
  56 #include "BLI_kdopbvh.h"
  57 #include "BKE_collision.h"
  58
  59 #include "DEG_depsgraph.h"
  60 #include "DEG_depsgraph_physics.h"
  61 #include "DEG_depsgraph_query.h"
  62
  63 #ifdef WITH_ELTOPO
  64 #include "eltopo-capi.h"
  65 #endif
  66
  67
  68 typedef struct ColDetectData {
  69         ClothModifierData *clmd;
  70         CollisionModifierData *collmd;
  71         BVHTreeOverlap *overlap;
  72         CollPair *collisions;
  73         bool culling;
  74         bool use_normal;
  75         bool collided;
  76 } ColDetectData;
  77
  78 typedef struct SelfColDetectData {
  79         ClothModifierData *clmd;
  80         BVHTreeOverlap *overlap;
  81         CollPair *collisions;
  82         bool collided;
  83 } SelfColDetectData;
  84
  85 /***********************************
  86 Collision modifier code start
  87 ***********************************/
  88
  89 /* step is limited from 0 (frame start position) to 1 (frame end position) */
  90 void collision_move_object(CollisionModifierData *collmd, float step, float prevstep)
  91 {
  92         float oldx[3];
  93         unsigned int i = 0;
  94
  95         /* the collider doesn't move this frame */
  96         if (collmd->is_static) {
  97                 for (i = 0; i < collmd->mvert_num; i++) {
  98                         zero_v3(collmd->current_v[i].co);
  99                 }
 100
 101                 return;
 102         }
 103
 104         for (i = 0; i < collmd->mvert_num; i++) {
 105                 interp_v3_v3v3(oldx, collmd->x[i].co, collmd->xnew[i].co, prevstep);
 106                 interp_v3_v3v3(collmd->current_x[i].co, collmd->x[i].co, collmd->xnew[i].co, step);
 107                 sub_v3_v3v3(collmd->current_v[i].co, collmd->current_x[i].co, oldx);
 108         }
 109
 110         bvhtree_update_from_mvert(
 111                 collmd->bvhtree, collmd->current_x, NULL,
 112                 collmd->tri, collmd->tri_num, false);
 113 }
 114
 115 BVHTree *bvhtree_build_from_mvert(
 116         const MVert *mvert,
 117         const struct MVertTri *tri, int tri_num,
 118         float epsilon)
 119 {
 120         BVHTree *tree;
 121         const MVertTri *vt;
 122         int i;
 123
 124         tree = BLI_bvhtree_new(tri_num, epsilon, 4, 26);
 125
 126         /* fill tree */
 127         for (i = 0, vt = tri; i < tri_num; i++, vt++) {
 128                 float co[3][3];
 129
 130                 copy_v3_v3(co[0], mvert[vt->tri[0]].co);
 131                 copy_v3_v3(co[1], mvert[vt->tri[1]].co);
 132                 copy_v3_v3(co[2], mvert[vt->tri[2]].co);
 133
 134                 BLI_bvhtree_insert(tree, i, co[0], 3);
 135         }
 136
 137         /* balance tree */
 138         BLI_bvhtree_balance(tree);
 139
 140         return tree;
 141 }
 142
 143 void bvhtree_update_from_mvert(
 144         BVHTree *bvhtree,
 145         const MVert *mvert, const MVert *mvert_moving,
 146         const MVertTri *tri, int tri_num,
 147         bool moving)
 148 {
 149         const MVertTri *vt;
 150         int i;
 151
 152         if ((bvhtree == NULL) || (mvert == NULL)) {
 153                 return;
 154         }
 155
 156         if (mvert_moving == NULL) {
 157                 moving = false;
 158         }
 159
 160         for (i = 0, vt = tri; i < tri_num; i++, vt++) {
 161                 float co[3][3];
 162                 bool ret;
 163
 164                 copy_v3_v3(co[0], mvert[vt->tri[0]].co);
 165                 copy_v3_v3(co[1], mvert[vt->tri[1]].co);
 166                 copy_v3_v3(co[2], mvert[vt->tri[2]].co);
 167
 168                 /* copy new locations into array */
 169                 if (moving) {
 170                         float co_moving[3][3];
 171                         /* update moving positions */
 172                         copy_v3_v3(co_moving[0], mvert_moving[vt->tri[0]].co);
 173                         copy_v3_v3(co_moving[1], mvert_moving[vt->tri[1]].co);
 174                         copy_v3_v3(co_moving[2], mvert_moving[vt->tri[2]].co);
 175
 176                         ret = BLI_bvhtree_update_node(bvhtree, i, &co[0][0], &co_moving[0][0], 3);
 177                 }
 178                 else {
 179                         ret = BLI_bvhtree_update_node(bvhtree, i, &co[0][0], NULL, 3);
 180                 }
 181
 182                 /* check if tree is already full */
 183                 if (ret == false) {
 184                         break;
 185                 }
 186         }
 187
 188         BLI_bvhtree_update_tree(bvhtree);
 189 }
 190
 191 /***********************************
 192 Collision modifier code end
 193 ***********************************/
 194
 195 BLI_INLINE int next_ind(int i)
 196 {
 197         return (++i < 3) ? i : 0;
 198 }
 199
 200 static float compute_collision_point(float a1[3], float a2[3], float a3[3], float b1[3], float b2[3], float b3[3],
 201                                      bool culling, bool use_normal, float r_a[3], float r_b[3], float r_vec[3])
 202 {
 203         float a[3][3];
 204         float b[3][3];
 205         float dist = FLT_MAX;
 206         float tmp_co1[3], tmp_co2[3];
 207         float isect_a[3], isect_b[3];
 208         int isect_count = 0;
 209         float tmp, tmp_vec[3];
 210         float normal[3], cent[3];
 211         bool backside = false;
 212
 213         copy_v3_v3(a[0], a1);
 214         copy_v3_v3(a[1], a2);
 215         copy_v3_v3(a[2], a3);
 216
 217         copy_v3_v3(b[0], b1);
 218         copy_v3_v3(b[1], b2);
 219         copy_v3_v3(b[2], b3);
 220
 221         /* Find intersections. */
 222         for (int i = 0; i < 3; i++) {
 223                 if (isect_line_segment_tri_v3(a[i], a[next_ind(i)], b[0], b[1], b[2], &tmp, NULL)) {
 224                         interp_v3_v3v3(isect_a, a[i], a[next_ind(i)], tmp);
 225                         isect_count++;
 226                 }
 227         }
 228
 229         if (isect_count == 0) {
 230                 for (int i = 0; i < 3; i++) {
 231                         if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) {
 232                                 isect_count++;
 233                         }
 234                 }
 235         }
 236         else if (isect_count == 1) {
 237                 for (int i = 0; i < 3; i++) {
 238                         if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) {
 239                                 interp_v3_v3v3(isect_b, b[i], b[next_ind(i)], tmp);
 240                                 break;
 241                         }
 242                 }
 243         }
 244
 245         /* Determine collision side. */
 246         if (culling) {
 247                 normal_tri_v3(normal, b[0], b[1], b[2]);
 248                 mid_v3_v3v3v3(cent, b[0], b[1], b[2]);
 249
 250                 if (isect_count == 2) {
 251                         backside = true;
 252                 }
 253                 else if (isect_count == 0) {
 254                         for (int i = 0; i < 3; i++) {
 255                                 sub_v3_v3v3(tmp_vec, a[i], cent);
 256                                 if (dot_v3v3(tmp_vec, normal) < 0.0f) {
 257                                         backside = true;
 258                                         break;
 259                                 }
 260                         }
 261                 }
 262         }
 263         else if (use_normal) {
 264                 normal_tri_v3(normal, b[0], b[1], b[2]);
 265         }
 266
 267         if (isect_count == 1) {
 268                 /* Edge intersection. */
 269                 copy_v3_v3(r_a, isect_a);
 270                 copy_v3_v3(r_b, isect_b);
 271
 272                 if (use_normal) {
 273                         copy_v3_v3(r_vec, normal);
 274                 }
 275                 else {
 276                         sub_v3_v3v3(r_vec, r_b, r_a);
 277                 }
 278
 279                 return 0.0f;
 280         }
 281
 282         if (backside) {
 283                 float maxdist = 0.0f;
 284                 bool found = false;
 285
 286                 /* Point projections. */
 287                 for (int i = 0; i < 3; i++) {
 288                         if (isect_ray_tri_v3(a[i], normal, b[0], b[1], b[2], &tmp, NULL)) {
 289                                 if (tmp > maxdist) {
 290                                         maxdist = tmp;
 291                                         copy_v3_v3(r_a, a[i]);
 292                                         madd_v3_v3v3fl(r_b, a[i], normal, tmp);
 293                                         found = true;
 294                                 }
 295                         }
 296                 }
 297
 298                 negate_v3(normal);
 299
 300                 for (int i = 0; i < 3; i++) {
 301                         if (isect_ray_tri_v3(b[i], normal, a[0], a[1], a[2], &tmp, NULL)) {
 302                                 if (tmp > maxdist) {
 303                                         maxdist = tmp;
 304                                         madd_v3_v3v3fl(r_a, b[i], normal, tmp);
 305                                         copy_v3_v3(r_b, b[i]);
 306                                         found = true;
 307                                 }
 308                         }
 309                 }
 310
 311                 negate_v3(normal);
 312
 313                 /* Edge projections. */
 314                 for (int i = 0; i < 3; i++) {
 315                         float dir[3];
 316
 317                         sub_v3_v3v3(tmp_vec, b[next_ind(i)], b[i]);
 318                         cross_v3_v3v3(dir, tmp_vec, normal);
 319
 320                         for (int j = 0; j < 3; j++) {
 321                                 if (isect_line_plane_v3(tmp_co1, a[j], a[next_ind(j)], b[i], dir) &&
 322                                     point_in_slice_seg(tmp_co1, a[j], a[next_ind(j)]) &&
 323                                     point_in_slice_seg(tmp_co1, b[i], b[next_ind(i)]))
 324                                 {
 325                                         closest_to_line_v3(tmp_co2, tmp_co1, b[i], b[next_ind(i)]);
 326                                         sub_v3_v3v3(tmp_vec, tmp_co1, tmp_co2);
 327                                         tmp = len_v3(tmp_vec);
 328
 329                                         if ((tmp > maxdist) && (dot_v3v3(tmp_vec, normal) < 0.0f)) {
 330                                                 maxdist = tmp;
 331                                                 copy_v3_v3(r_a, tmp_co1);
 332                                                 copy_v3_v3(r_b, tmp_co2);
 333                                                 found = true;
 334                                         }
 335                                 }
 336                         }
 337                 }
 338
 339                 /* If no point is found, will fallback onto regular proximity test below. */
 340                 if (found) {
 341                         sub_v3_v3v3(r_vec, r_b, r_a);
 342
 343                         if (use_normal) {
 344                                 if (dot_v3v3(normal, r_vec) >= 0.0f) {
 345                                         copy_v3_v3(r_vec, normal);
 346                                 }
 347                                 else {
 348                                         negate_v3_v3(r_vec, normal);
 349                                 }
 350                         }
 351
 352                         return 0.0f;
 353                 }
 354         }
 355
 356         /* Closest point. */
 357         for (int i = 0; i < 3; i++) {
 358                 closest_on_tri_to_point_v3(tmp_co1, a[i], b[0], b[1], b[2]);
 359                 tmp = len_squared_v3v3(tmp_co1, a[i]);
 360
 361                 if (tmp < dist) {
 362                         dist = tmp;
 363                         copy_v3_v3(r_a, a[i]);
 364                         copy_v3_v3(r_b, tmp_co1);
 365                 }
 366         }
 367
 368         for (int i = 0; i < 3; i++) {
 369                 closest_on_tri_to_point_v3(tmp_co1, b[i], a[0], a[1], a[2]);
 370                 tmp = len_squared_v3v3(tmp_co1, b[i]);
 371
 372                 if (tmp < dist) {
 373                         dist = tmp;
 374                         copy_v3_v3(r_a, tmp_co1);
 375                         copy_v3_v3(r_b, b[i]);
 376                 }
 377         }
 378
 379         /* Closest edge. */
 380         if (isect_count == 0) {
 381                 for (int i = 0; i < 3; i++) {
 382                         for (int j = 0; j < 3; j++) {
 383                                 isect_seg_seg_v3(a[i], a[next_ind(i)], b[j], b[next_ind(j)], tmp_co1, tmp_co2);
 384                                 tmp = len_squared_v3v3(tmp_co1, tmp_co2);
 385
 386                                 if (tmp < dist) {
 387                                         dist = tmp;
 388                                         copy_v3_v3(r_a, tmp_co1);
 389                                         copy_v3_v3(r_b, tmp_co2);
 390                                 }
 391                         }
 392                 }
 393         }
 394
 395         if (isect_count == 0) {
 396                 sub_v3_v3v3(r_vec, r_a, r_b);
 397                 dist = sqrtf(dist);
 398         }
 399         else {
 400                 sub_v3_v3v3(r_vec, r_b, r_a);
 401                 dist = 0.0f;
 402         }
 403
 404         if (culling && use_normal) {
 405                 copy_v3_v3(r_vec, normal);
 406         }
 407         else if (use_normal) {
 408                 if (dot_v3v3(normal, r_vec) >= 0.0f) {
 409                         copy_v3_v3(r_vec, normal);
 410                 }
 411                 else {
 412                         negate_v3_v3(r_vec, normal);
 413                 }
 414         }
 415         else if (culling && (dot_v3v3(r_vec, normal) < 0.0f)) {
 416                 return FLT_MAX;
 417         }
 418
 419         return dist;
 420 }
 421
 422 // w3 is not perfect
 423 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
 424 {
 425         /* dot_v3v3 */
 426 #define INPR(v1, v2) ( (v1)[0] * (v2)[0] + (v1)[1] * (v2)[1] + (v1)[2] * (v2)[2])
 427
 428         double tempV1[3], tempV2[3], tempV4[3];
 429         double a, b, c, d, e, f;
 430
 431         VECSUB ( tempV1, p1, p3 );
 432         VECSUB ( tempV2, p2, p3 );
 433         VECSUB ( tempV4, pv, p3 );
 434
 435         a = INPR ( tempV1, tempV1 );
 436         b = INPR ( tempV1, tempV2 );
 437         c = INPR ( tempV2, tempV2 );
 438         e = INPR ( tempV1, tempV4 );
 439         f = INPR ( tempV2, tempV4 );
 440
 441         d = ( a * c - b * b );
 442
 443         if ( ABS ( d ) < (double)ALMOST_ZERO ) {
 444                 *w1 = *w2 = *w3 = 1.0 / 3.0;
 445                 return;
 446         }
 447
 448         w1[0] = ( float ) ( ( e * c - b * f ) / d );
 449
 450         if ( w1[0] < 0 )
 451                 w1[0] = 0;
 452
 453         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
 454
 455         if ( w2[0] < 0 )
 456                 w2[0] = 0;
 457
 458         w3[0] = 1.0f - w1[0] - w2[0];
 459
 460 #undef INPR
 461 }
 462
 463 #ifdef __GNUC__
 464 #  pragma GCC diagnostic push
 465 #  pragma GCC diagnostic ignored "-Wdouble-promotion"
 466 #endif
 467
 468 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
 469 {
 470         zero_v3(to);
 471         VECADDMUL(to, v1, w1);
 472         VECADDMUL(to, v2, w2);
 473         VECADDMUL(to, v3, w3);
 474 }
 475
 476 static int cloth_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, Object *collob,
 477                                            CollPair *collpair, uint collision_count, const float dt)
 478 {
 479         int result = 0;
 480         Cloth *cloth1;
 481         float w1, w2, w3, u1, u2, u3;
 482         float v1[3], v2[3], relativeVelocity[3];
 483         float magrelVel;
 484         float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
 485
 486         cloth1 = clmd->clothObject;
 487
 488         for (int i = 0; i < collision_count; i++, collpair++) {
 489                 float i1[3], i2[3], i3[3];
 490
 491                 zero_v3(i1);
 492                 zero_v3(i2);
 493                 zero_v3(i3);
 494
 495                 /* Only handle static collisions here. */
 496                 if (collpair->flag & (COLLISION_IN_FUTURE | COLLISION_INACTIVE)) {
 497                         continue;
 498                 }
 499
 500                 /* Compute barycentric coordinates for both collision points. */
 501                 collision_compute_barycentric(collpair->pa,
 502                                               cloth1->verts[collpair->ap1].tx,
 503                                               cloth1->verts[collpair->ap2].tx,
 504                                               cloth1->verts[collpair->ap3].tx,
 505                                               &w1, &w2, &w3);
 506
 507                 collision_compute_barycentric(collpair->pb,
 508                                               collmd->current_x[collpair->bp1].co,
 509                                               collmd->current_x[collpair->bp2].co,
 510                                               collmd->current_x[collpair->bp3].co,
 511                                               &u1, &u2, &u3);
 512
 513                 /* Calculate relative "velocity". */
 514                 collision_interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
 515
 516                 collision_interpolateOnTriangle(v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3);
 517
 518                 sub_v3_v3v3(relativeVelocity, v2, v1);
 519
 520                 /* Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). */
 521                 magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
 522
 523                 /* If magrelVel < 0 the edges are approaching each other. */
 524                 if (magrelVel > 0.0f) {
 525                         /* Calculate Impulse magnitude to stop all motion in normal direction. */
 526                         float magtangent = 0, repulse = 0, d = 0;
 527                         double impulse = 0.0;
 528                         float vrel_t_pre[3];
 529                         float temp[3];
 530                         float time_multiplier;
 531
 532                         /* Calculate tangential velocity. */
 533                         copy_v3_v3(temp, collpair->normal);
 534                         mul_v3_fl(temp, magrelVel);
 535                         sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
 536
 537                         /* Decrease in magnitude of relative tangential velocity due to coulomb friction
 538                          * in original formula "magrelVel" should be the "change of relative velocity in normal direction". */
 539                         magtangent = min_ff(collob->pd->pdef_cfrict * 0.01f * magrelVel, len_v3(vrel_t_pre));
 540
 541                         /* Apply friction impulse. */
 542                         if ( magtangent > ALMOST_ZERO ) {
 543                                 normalize_v3(vrel_t_pre);
 544
 545                                 impulse = magtangent / 1.5;
 546
 547                                 VECADDMUL(i1, vrel_t_pre, w1 * impulse);
 548                                 VECADDMUL(i2, vrel_t_pre, w2 * impulse);
 549                                 VECADDMUL(i3, vrel_t_pre, w3 * impulse);
 550                         }
 551
 552                         /* Apply velocity stopping impulse. */
 553                         impulse =  magrelVel / 1.5f;
 554
 555                         VECADDMUL(i1, collpair->normal, w1 * impulse);
 556                         cloth1->verts[collpair->ap1].impulse_count++;
 557
 558                         VECADDMUL(i2, collpair->normal, w2 * impulse);
 559                         cloth1->verts[collpair->ap2].impulse_count++;
 560
 561                         VECADDMUL(i3, collpair->normal, w3 * impulse);
 562                         cloth1->verts[collpair->ap3].impulse_count++;
 563
 564                         time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
 565
 566                         d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
 567
 568                         if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) {
 569                                 repulse = MIN2(d / time_multiplier, 0.1f * d * time_multiplier - magrelVel);
 570
 571                                 /* Stay on the safe side and clamp repulse. */
 572                                 if (impulse > ALMOST_ZERO) {
 573                                         repulse = min_ff(repulse, 5.0f * impulse);
 574                                 }
 575
 576                                 repulse = max_ff(impulse, repulse);
 577
 578                                 impulse = repulse / 1.5f;
 579
 580                                 VECADDMUL(i1, collpair->normal, impulse);
 581                                 VECADDMUL(i2, collpair->normal, impulse);
 582                                 VECADDMUL(i3, collpair->normal, impulse);
 583                         }
 584
 585                         result = 1;
 586                 }
 587                 else {
 588                         float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
 589                         float d;
 590
 591                         d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
 592
 593                         if (d > ALMOST_ZERO) {
 594                                 /* Stay on the safe side and clamp repulse. */
 595                                 float repulse = d / time_multiplier;
 596                                 float impulse = repulse / 4.5f;
 597
 598                                 VECADDMUL(i1, collpair->normal, w1 * impulse);
 599                                 VECADDMUL(i2, collpair->normal, w2 * impulse);
 600                                 VECADDMUL(i3, collpair->normal, w3 * impulse);
 601
 602                                 cloth1->verts[collpair->ap1].impulse_count++;
 603                                 cloth1->verts[collpair->ap2].impulse_count++;
 604                                 cloth1->verts[collpair->ap3].impulse_count++;
 605
 606                                 result = 1;
 607                         }
 608                 }
 609
 610                 if (result) {
 611                         float clamp = clmd->coll_parms->clamp * dt;
 612
 613                         if ((clamp > 0.0f) &&
 614                             ((len_v3(i1) > clamp) ||
 615                              (len_v3(i2) > clamp) ||
 616                              (len_v3(i3) > clamp)))
 617                         {
 618                                 return 0;
 619                         }
 620
 621                         for (int j = 0; j < 3; j++) {
 622                                 if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j]))
 623                                         cloth1->verts[collpair->ap1].impulse[j] = i1[j];
 624
 625                                 if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j]))
 626                                         cloth1->verts[collpair->ap2].impulse[j] = i2[j];
 627
 628                                 if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j]))
 629                                         cloth1->verts[collpair->ap3].impulse[j] = i3[j];
 630                         }
 631                 }
 632         }
 633
 634         return result;
 635 }
 636
 637 static int cloth_selfcollision_response_static(ClothModifierData *clmd, CollPair *collpair,
 638                                                uint collision_count, const float dt)
 639 {
 640         int result = 0;
 641         Cloth *cloth1;
 642         float w1, w2, w3, u1, u2, u3;
 643         float v1[3], v2[3], relativeVelocity[3];
 644         float magrelVel;
 645
 646         cloth1 = clmd->clothObject;
 647
 648         for (int i = 0; i < collision_count; i++, collpair++) {
 649                 float i1[3], i2[3], i3[3];
 650
 651                 zero_v3(i1);
 652                 zero_v3(i2);
 653                 zero_v3(i3);
 654
 655                 /* Only handle static collisions here. */
 656                 if (collpair->flag & (COLLISION_IN_FUTURE | COLLISION_INACTIVE)) {
 657                         continue;
 658                 }
 659
 660                 /* Compute barycentric coordinates for both collision points. */
 661                 collision_compute_barycentric(collpair->pa,
 662                                               cloth1->verts[collpair->ap1].tx,
 663                                               cloth1->verts[collpair->ap2].tx,
 664                                               cloth1->verts[collpair->ap3].tx,
 665                                               &w1, &w2, &w3);
 666
 667                 collision_compute_barycentric(collpair->pb,
 668                                               cloth1->verts[collpair->bp1].tx,
 669                                               cloth1->verts[collpair->bp2].tx,
 670                                               cloth1->verts[collpair->bp3].tx,
 671                                               &u1, &u2, &u3);
 672
 673                 /* Calculate relative "velocity". */
 674                 collision_interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
 675
 676                 collision_interpolateOnTriangle(v2, cloth1->verts[collpair->bp1].tv, cloth1->verts[collpair->bp2].tv, cloth1->verts[collpair->bp3].tv, u1, u2, u3);
 677
 678                 sub_v3_v3v3(relativeVelocity, v2, v1);
 679
 680                 /* Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). */
 681                 magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
 682
 683                 /* TODO: Impulses should be weighed by mass as this is self col,
 684                  * this has to be done after mass distribution is implemented. */
 685
 686                 /* If magrelVel < 0 the edges are approaching each other. */
 687                 if (magrelVel > 0.0f) {
 688                         /* Calculate Impulse magnitude to stop all motion in normal direction. */
 689                         float magtangent = 0, repulse = 0, d = 0;
 690                         double impulse = 0.0;
 691                         float vrel_t_pre[3];
 692                         float temp[3], time_multiplier;
 693
 694                         /* Calculate tangential velocity. */
 695                         copy_v3_v3(temp, collpair->normal);
 696                         mul_v3_fl(temp, magrelVel);
 697                         sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
 698
 699                         /* Decrease in magnitude of relative tangential velocity due to coulomb friction
 700                          * in original formula "magrelVel" should be the "change of relative velocity in normal direction". */
 701                         magtangent = min_ff(clmd->coll_parms->self_friction * 0.01f * magrelVel, len_v3(vrel_t_pre));
 702
 703                         /* Apply friction impulse. */
 704                         if (magtangent > ALMOST_ZERO) {
 705                                 normalize_v3(vrel_t_pre);
 706
 707                                 impulse = magtangent / 1.5;
 708
 709                                 VECADDMUL(i1, vrel_t_pre, w1 * impulse);
 710                                 VECADDMUL(i2, vrel_t_pre, w2 * impulse);
 711                                 VECADDMUL(i3, vrel_t_pre, w3 * impulse);
 712                         }
 713
 714                         /* Apply velocity stopping impulse. */
 715                         impulse = magrelVel / 3.0f;
 716
 717                         VECADDMUL(i1, collpair->normal, w1 * impulse);
 718                         cloth1->verts[collpair->ap1].impulse_count++;
 719
 720                         VECADDMUL(i2, collpair->normal, w2 * impulse);
 721                         cloth1->verts[collpair->ap2].impulse_count++;
 722
 723                         VECADDMUL(i3, collpair->normal, w3 * impulse);
 724                         cloth1->verts[collpair->ap3].impulse_count++;
 725
 726                         time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
 727
 728                         d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance;
 729
 730                         if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) {
 731                                 repulse = MIN2 (d / time_multiplier, 0.1f * d * time_multiplier - magrelVel);
 732
 733                                 if (impulse > ALMOST_ZERO) {
 734                                         repulse = min_ff(repulse, 5.0*impulse);
 735                                 }
 736
 737                                 repulse = max_ff(impulse, repulse);
 738
 739                                 impulse = repulse / 1.5f;
 740
 741                                 VECADDMUL(i1, collpair->normal, w1 * impulse);
 742                                 VECADDMUL(i2, collpair->normal, w2 * impulse);
 743                                 VECADDMUL(i3, collpair->normal, w3 * impulse);
 744                         }
 745
 746                         result = 1;
 747                 }
 748                 else {
 749                         float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
 750                         float d;
 751
 752                         d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance;
 753
 754                         if ( d > ALMOST_ZERO) {
 755                                 /* Stay on the safe side and clamp repulse. */
 756                                 float repulse = d*1.0f/time_multiplier;
 757                                 float impulse = repulse / 9.0f;
 758
 759                                 VECADDMUL(i1, collpair->normal, w1 * impulse);
 760                                 VECADDMUL(i2, collpair->normal, w2 * impulse);
 761                                 VECADDMUL(i3, collpair->normal, w3 * impulse);
 762
 763                                 cloth1->verts[collpair->ap1].impulse_count++;
 764                                 cloth1->verts[collpair->ap2].impulse_count++;
 765                                 cloth1->verts[collpair->ap3].impulse_count++;
 766
 767                                 result = 1;
 768                         }
 769                 }
 770
 771                 if (result) {
 772                         float clamp = clmd->coll_parms->self_clamp * dt;
 773
 774                         if ((clamp > 0.0f) &&
 775                             ((len_v3(i1) > clamp) ||
 776                              (len_v3(i2) > clamp) ||
 777                              (len_v3(i3) > clamp)))
 778                         {
 779                                 return 0;
 780                         }
 781
 782                         for (int j = 0; j < 3; j++) {
 783                                 if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j]))
 784                                         cloth1->verts[collpair->ap1].impulse[j] = i1[j];
 785
 786                                 if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j]))
 787                                         cloth1->verts[collpair->ap2].impulse[j] = i2[j];
 788
 789                                 if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j]))
 790                                         cloth1->verts[collpair->ap3].impulse[j] = i3[j];
 791                         }
 792                 }
 793         }
 794
 795         return result;
 796 }
 797
 798 #ifdef __GNUC__
 799 #  pragma GCC diagnostic pop
 800 #endif
 801
 802 static void cloth_collision(
 803         void *__restrict userdata,
 804         const int index,
 805         const ParallelRangeTLS *__restrict UNUSED(tls))
 806 {
 807         ColDetectData *data = (ColDetectData *)userdata;
 808
 809         ClothModifierData *clmd = data->clmd;
 810         CollisionModifierData *collmd = data->collmd;
 811         CollPair *collpair = data->collisions;
 812         const MVertTri *tri_a, *tri_b;
 813         ClothVertex *verts1 = clmd->clothObject->verts;
 814         float distance = 0.0f;
 815         float epsilon1 = clmd->coll_parms->epsilon;
 816         float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
 817         float pa[3], pb[3], vect[3];
 818
 819         tri_a = &clmd->clothObject->tri[data->overlap[index].indexA];
 820         tri_b = &collmd->tri[data->overlap[index].indexB];
 821
 822         /* Compute distance and normal. */
 823         distance = compute_collision_point(verts1[tri_a->tri[0]].tx, verts1[tri_a->tri[1]].tx, verts1[tri_a->tri[2]].tx,
 824                                            collmd->current_x[tri_b->tri[0]].co, collmd->current_x[tri_b->tri[1]].co, collmd->current_x[tri_b->tri[2]].co,
 825                                            data->culling, data->use_normal, pa, pb, vect);
 826
 827         if ((distance <= (epsilon1 + epsilon2 + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) {
 828                 collpair[index].ap1 = tri_a->tri[0];
 829                 collpair[index].ap2 = tri_a->tri[1];
 830                 collpair[index].ap3 = tri_a->tri[2];
 831
 832                 collpair[index].bp1 = tri_b->tri[0];
 833                 collpair[index].bp2 = tri_b->tri[1];
 834                 collpair[index].bp3 = tri_b->tri[2];
 835
 836                 copy_v3_v3(collpair[index].pa, pa);
 837                 copy_v3_v3(collpair[index].pb, pb);
 838                 copy_v3_v3(collpair[index].vector, vect);
 839
 840                 normalize_v3_v3(collpair[index].normal, collpair[index].vector);
 841
 842                 collpair[index].distance = distance;
 843                 collpair[index].flag = 0;
 844
 845                 data->collided = true;
 846         }
 847         else {
 848                 collpair[index].flag = COLLISION_INACTIVE;
 849         }
 850 }
 851
 852 static void cloth_selfcollision(
 853         void *__restrict userdata,
 854         const int index,
 855         const ParallelRangeTLS *__restrict UNUSED(tls))
 856 {
 857         SelfColDetectData *data = (SelfColDetectData *)userdata;
 858
 859         ClothModifierData *clmd = data->clmd;
 860         CollPair *collpair = data->collisions;
 861         const MVertTri *tri_a, *tri_b;
 862         ClothVertex *verts1 = clmd->clothObject->verts;
 863         float distance = 0.0f;
 864         float epsilon = clmd->coll_parms->selfepsilon;
 865         float pa[3], pb[3], vect[3];
 866
 867         tri_a = &clmd->clothObject->tri[data->overlap[index].indexA];
 868         tri_b = &clmd->clothObject->tri[data->overlap[index].indexB];
 869
 870         for (uint i = 0; i < 3; i++) {
 871                 for (uint j = 0; j < 3; j++) {
 872                         if (tri_a->tri[i] == tri_b->tri[j]) {
 873                                 collpair[index].flag = COLLISION_INACTIVE;
 874                                 return;
 875                         }
 876                 }
 877         }
 878
 879         if (((verts1[tri_a->tri[0]].flags & verts1[tri_a->tri[1]].flags & verts1[tri_a->tri[2]].flags) |
 880              (verts1[tri_b->tri[0]].flags & verts1[tri_b->tri[1]].flags & verts1[tri_b->tri[2]].flags)) & CLOTH_VERT_FLAG_NOSELFCOLL)
 881         {
 882                 collpair[index].flag = COLLISION_INACTIVE;
 883                 return;
 884         }
 885
 886         /* Compute distance and normal. */
 887         distance = compute_collision_point(verts1[tri_a->tri[0]].tx, verts1[tri_a->tri[1]].tx, verts1[tri_a->tri[2]].tx,
 888                                            verts1[tri_b->tri[0]].tx, verts1[tri_b->tri[1]].tx, verts1[tri_b->tri[2]].tx,
 889                                            false, false, pa, pb, vect);
 890
 891         if ((distance <= (epsilon * 2.0f + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) {
 892                 collpair[index].ap1 = tri_a->tri[0];
 893                 collpair[index].ap2 = tri_a->tri[1];
 894                 collpair[index].ap3 = tri_a->tri[2];
 895
 896                 collpair[index].bp1 = tri_b->tri[0];
 897                 collpair[index].bp2 = tri_b->tri[1];
 898                 collpair[index].bp3 = tri_b->tri[2];
 899
 900                 copy_v3_v3(collpair[index].pa, pa);
 901                 copy_v3_v3(collpair[index].pb, pb);
 902                 copy_v3_v3(collpair[index].vector, vect);
 903
 904                 normalize_v3_v3(collpair[index].normal, collpair[index].vector);
 905
 906                 collpair[index].distance = distance;
 907                 collpair[index].flag = 0;
 908
 909                 data->collided = true;
 910         }
 911         else {
 912                 collpair[index].flag = COLLISION_INACTIVE;
 913         }
 914 }
 915
 916 static void add_collision_object(ListBase *relations, Object *ob, int level, unsigned int modifier_type)
 917 {
 918         CollisionModifierData *cmd= NULL;
 919
 920         /* only get objects with collision modifier */
 921         if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) || (modifier_type != eModifierType_Collision))
 922                 cmd= (CollisionModifierData *)modifiers_findByType(ob, modifier_type);
 923
 924         if (cmd) {
 925                 CollisionRelation *relation = MEM_callocN(sizeof(CollisionRelation), "CollisionRelation");
 926                 relation->ob = ob;
 927                 BLI_addtail(relations, relation);
 928         }
 929
 930         /* objects in dupli groups, one level only for now */
 931         /* TODO: this doesn't really work, we are not taking into account the
 932          * dupli transforms and can get objects in the list multiple times. */
 933         if (ob->dup_group && level == 0) {
 934                 Collection *collection= ob->dup_group;
 935
 936                 /* add objects */
 937                 FOREACH_COLLECTION_OBJECT_RECURSIVE_BEGIN(collection, object)
 938                 {
 939                         add_collision_object(relations, object, level+1, modifier_type);
 940                 }
 941                 FOREACH_COLLECTION_OBJECT_RECURSIVE_END;
 942         }
 943 }
 944
 945 /* Create list of collision relations in the collection or entire scene.
 946  * This is used by the depsgraph to build relations, as well as faster
 947  * lookup of colliders during evaluation. */
 948 ListBase *BKE_collision_relations_create(Depsgraph *depsgraph, Collection *collection, unsigned int modifier_type)
 949 {
 950         ViewLayer *view_layer = DEG_get_input_view_layer(depsgraph);
 951         Base *base = BKE_collection_or_layer_objects(view_layer, collection);
 952         const bool for_render = (DEG_get_mode(depsgraph) == DAG_EVAL_RENDER);
 953         const int base_flag = (for_render) ? BASE_ENABLED_RENDER : BASE_ENABLED_VIEWPORT;
 954
 955         ListBase *relations = MEM_callocN(sizeof(ListBase), "CollisionRelation list");
 956
 957         for (; base; base = base->next) {
 958                 if (base->flag & base_flag) {
 959                         add_collision_object(relations, base->object, 0, modifier_type);
 960                 }
 961         }
 962
 963         return relations;
 964 }
 965
 966 void BKE_collision_relations_free(ListBase *relations)
 967 {
 968         if (relations) {
 969                 BLI_freelistN(relations);
 970                 MEM_freeN(relations);
 971         }
 972 }
 973
 974 /* Create effective list of colliders from relations built beforehand.
 975  * Self will be excluded. */
 976 Object **BKE_collision_objects_create(Depsgraph *depsgraph, Object *self, Collection *collection, unsigned int *numcollobj, unsigned int modifier_type)
 977 {
 978         ListBase *relations = DEG_get_collision_relations(depsgraph, collection, modifier_type);
 979
 980         if (!relations) {
 981                 *numcollobj = 0;
 982                 return NULL;
 983         }
 984
 985         int maxnum = BLI_listbase_count(relations);
 986         int num = 0;
 987         Object **objects = MEM_callocN(sizeof(Object*) * maxnum, __func__);
 988
 989         for (CollisionRelation *relation = relations->first; relation; relation = relation->next) {
 990                 /* Get evaluated object. */
 991                 Object *ob = (Object*)DEG_get_evaluated_id(depsgraph, &relation->ob->id);
 992
 993                 if (ob != self) {
 994                         objects[num] = ob;
 995                         num++;
 996                 }
 997         }
 998
 999         if (num == 0) {
1000                 MEM_freeN(objects);
1001                 objects = NULL;
1002         }
1003
1004         *numcollobj = num;
1005         return objects;
1006 }
1007
1008 void BKE_collision_objects_free(Object **objects)
1009 {
1010         if (objects) {
1011                 MEM_freeN(objects);
1012         }
1013 }
1014
1015 /* Create effective list of colliders from relations built beforehand.
1016  * Self will be excluded. */
1017 ListBase *BKE_collider_cache_create(Depsgraph *depsgraph, Object *self, Collection *collection)
1018 {
1019         ListBase *relations = DEG_get_collision_relations(depsgraph, collection, eModifierType_Collision);
1020         ListBase *cache = NULL;
1021
1022         if (!relations) {
1023                 return NULL;
1024         }
1025
1026         for (CollisionRelation *relation = relations->first; relation; relation = relation->next) {
1027                 /* Get evaluated object. */
1028                 Object *ob = (Object*)DEG_get_evaluated_id(depsgraph, &relation->ob->id);
1029
1030                 if (ob == self) {
1031                         continue;
1032                 }
1033
1034                 CollisionModifierData *cmd = (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
1035                 if (cmd && cmd->bvhtree) {
1036                         if (cache == NULL) {
1037                                 cache = MEM_callocN(sizeof(ListBase), "ColliderCache array");
1038                         }
1039
1040                         ColliderCache *col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
1041                         col->ob = ob;
1042                         col->collmd = cmd;
1043                         /* make sure collider is properly set up */
1044                         collision_move_object(cmd, 1.0, 0.0);
1045                         BLI_addtail(cache, col);
1046                 }
1047         }
1048
1049         return cache;
1050 }
1051
1052 void BKE_collider_cache_free(ListBase **colliders)
1053 {
1054         if (*colliders) {
1055                 BLI_freelistN(*colliders);
1056                 MEM_freeN(*colliders);
1057                 *colliders = NULL;
1058         }
1059 }
1060
1061 static bool cloth_bvh_objcollisions_nearcheck(ClothModifierData * clmd, CollisionModifierData *collmd,
1062                                               CollPair **collisions, int numresult,
1063                                               BVHTreeOverlap *overlap, bool culling, bool use_normal)
1064 {
1065         *collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * numresult, "collision array");
1066
1067         ColDetectData data = {.clmd = clmd,
1068                               .collmd = collmd,
1069                               .overlap = overlap,
1070                               .collisions = *collisions,
1071                               .culling = culling,
1072                               .use_normal = use_normal,
1073                               .collided = false};
1074
1075         ParallelRangeSettings settings;
1076         BLI_parallel_range_settings_defaults(&settings);
1077         settings.use_threading = true;
1078         BLI_task_parallel_range(0, numresult, &data, cloth_collision, &settings);
1079
1080         return data.collided;
1081 }
1082
1083 static bool cloth_bvh_selfcollisions_nearcheck(ClothModifierData * clmd, CollPair *collisions,
1084                                                int numresult, BVHTreeOverlap *overlap)
1085 {
1086         SelfColDetectData data = {.clmd = clmd,
1087                                   .overlap = overlap,
1088                                   .collisions = collisions,
1089                                   .collided = false};
1090
1091         ParallelRangeSettings settings;
1092         BLI_parallel_range_settings_defaults(&settings);
1093         settings.use_threading = true;
1094         BLI_task_parallel_range(0, numresult, &data, cloth_selfcollision, &settings);
1095
1096         return data.collided;
1097 }
1098
1099 static int cloth_bvh_objcollisions_resolve(ClothModifierData * clmd, Object **collobjs, CollPair **collisions,
1100                                            uint *collision_counts, const uint numcollobj, const float dt)
1101 {
1102         Cloth *cloth = clmd->clothObject;
1103         int i = 0, j = 0, mvert_num = 0;
1104         ClothVertex *verts = NULL;
1105         int ret = 0;
1106         int result = 0;
1107
1108         mvert_num = clmd->clothObject->mvert_num;
1109         verts = cloth->verts;
1110
1111         result = 1;
1112
1113         for (j = 0; j < 2; j++) {
1114                 result = 0;
1115
1116                 for (i = 0; i < numcollobj; i++) {
1117                         Object *collob= collobjs[i];
1118                         CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1119
1120                         if ( collmd->bvhtree ) {
1121                                 result += cloth_collision_response_static(clmd, collmd, collob, collisions[i], collision_counts[i], dt);
1122                         }
1123                 }
1124
1125                 /* Apply impulses in parallel. */
1126                 if (result) {
1127                         for (i = 0; i < mvert_num; i++) {
1128                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
1129                                 if (verts[i].impulse_count) {
1130                                         VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
1131                                         VECADD ( verts[i].dcvel, verts[i].dcvel, verts[i].impulse);
1132                                         zero_v3(verts[i].impulse);
1133                                         verts[i].impulse_count = 0;
1134
1135                                         ret++;
1136                                 }
1137                         }
1138                 }
1139                 else {
1140                         break;
1141                 }
1142         }
1143         return ret;
1144 }
1145
1146 static int cloth_bvh_selfcollisions_resolve(ClothModifierData * clmd, CollPair *collisions, int collision_count, const float dt)
1147 {
1148         Cloth *cloth = clmd->clothObject;
1149         int i = 0, j = 0, mvert_num = 0;
1150         ClothVertex *verts = NULL;
1151         int ret = 0;
1152         int result = 0;
1153
1154         mvert_num = clmd->clothObject->mvert_num;
1155         verts = cloth->verts;
1156
1157         for (j = 0; j < 2; j++) {
1158                 result = 0;
1159
1160                 result += cloth_selfcollision_response_static(clmd, collisions, collision_count, dt);
1161
1162                 /* Apply impulses in parallel. */
1163                 if (result) {
1164                         for (i = 0; i < mvert_num; i++) {
1165                                 if (verts[i].impulse_count) {
1166                                         // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1167                                         VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
1168                                         VECADD ( verts[i].dcvel, verts[i].dcvel, verts[i].impulse);
1169                                         zero_v3(verts[i].impulse);
1170                                         verts[i].impulse_count = 0;
1171
1172                                         ret++;
1173                                 }
1174                         }
1175                 }
1176
1177                 if (!result) {
1178                         break;
1179                 }
1180         }
1181         return ret;
1182 }
1183
1184 int cloth_bvh_collision(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt)
1185 {
1186         Cloth *cloth = clmd->clothObject;
1187         BVHTree *cloth_bvh = cloth->bvhtree;
1188         uint i = 0, mvert_num = 0;
1189         int rounds = 0;
1190         ClothVertex *verts = NULL;
1191         int ret = 0, ret2 = 0;
1192         Object **collobjs = NULL;
1193         unsigned int numcollobj = 0;
1194         uint *coll_counts_obj = NULL;
1195         BVHTreeOverlap **overlap_obj = NULL;
1196         uint coll_count_self = 0;
1197         BVHTreeOverlap *overlap_self = NULL;
1198
1199         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
1200                 return 0;
1201
1202         verts = cloth->verts;
1203         mvert_num = cloth->mvert_num;
1204
1205         if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
1206                 bvhtree_update_from_cloth(clmd, false, false);
1207
1208                 collobjs = BKE_collision_objects_create(depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
1209
1210                 if (collobjs) {
1211                         coll_counts_obj = MEM_callocN(sizeof(uint) * numcollobj, "CollCounts");
1212                         overlap_obj = MEM_callocN(sizeof(*overlap_obj) * numcollobj, "BVHOverlap");
1213
1214                         for (i = 0; i < numcollobj; i++) {
1215                                 Object *collob = collobjs[i];
1216                                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1217
1218                                 if (!collmd->bvhtree) {
1219                                         continue;
1220                                 }
1221
1222                                 /* Move object to position (step) in time. */
1223                                 collision_move_object(collmd, step + dt, step);
1224
1225                                 overlap_obj[i] = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &coll_counts_obj[i], NULL, NULL);
1226                         }
1227                 }
1228         }
1229
1230         if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) {
1231                 bvhtree_update_from_cloth(clmd, false, true);
1232
1233                 overlap_self = BLI_bvhtree_overlap(cloth->bvhselftree, cloth->bvhselftree, &coll_count_self, NULL, NULL);
1234         }
1235
1236         do {
1237                 ret2 = 0;
1238
1239                 /* Object collisions. */
1240                 if ((clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) && collobjs) {
1241                         CollPair **collisions;
1242                         bool collided = false;
1243
1244                         collisions = MEM_callocN(sizeof(CollPair *) * numcollobj, "CollPair");
1245
1246                         for (i = 0; i < numcollobj; i++) {
1247                                 Object *collob = collobjs[i];
1248                                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1249
1250                                 if (!collmd->bvhtree) {
1251                                         continue;
1252                                 }
1253
1254                                 if (coll_counts_obj[i] && overlap_obj[i]) {
1255                                         collided = cloth_bvh_objcollisions_nearcheck(clmd, collmd, &collisions[i], coll_counts_obj[i], overlap_obj[i],
1256                                                                                      (collob->pd->flag & PFIELD_CLOTH_USE_CULLING),
1257                                                                                      (collob->pd->flag & PFIELD_CLOTH_USE_NORMAL)) || collided;
1258                                 }
1259                         }
1260
1261                         if (collided) {
1262                                 ret += cloth_bvh_objcollisions_resolve(clmd, collobjs, collisions, coll_counts_obj, numcollobj, dt);
1263                                 ret2 += ret;
1264                         }
1265
1266                         for (i = 0; i < numcollobj; i++) {
1267                                 MEM_SAFE_FREE(collisions[i]);
1268                         }
1269
1270                         MEM_freeN(collisions);
1271                 }
1272
1273                 /* Self collisions. */
1274                 if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) {
1275                         CollPair *collisions = NULL;
1276
1277                         verts = cloth->verts;
1278                         mvert_num = cloth->mvert_num;
1279
1280                         if (cloth->bvhselftree) {
1281                                 if (coll_count_self && overlap_self) {
1282                                         collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * coll_count_self, "collision array");
1283
1284                                         if (cloth_bvh_selfcollisions_nearcheck(clmd, collisions, coll_count_self, overlap_self)) {
1285                                                 ret += cloth_bvh_selfcollisions_resolve(clmd, collisions,  coll_count_self, dt);
1286                                                 ret2 += ret;
1287                                         }
1288                                 }
1289
1290                         }
1291
1292                         MEM_SAFE_FREE(collisions);
1293                 }
1294
1295                 /* Apply all collision resolution. */
1296                 if (ret2) {
1297                         for (i = 0; i < mvert_num; i++) {
1298                                 if (clmd->sim_parms->vgroup_mass > 0) {
1299                                         if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
1300                                                 continue;
1301                                         }
1302                                 }
1303
1304                                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1305                         }
1306                 }
1307
1308                 rounds++;
1309         }
1310         while (ret2 && (clmd->coll_parms->loop_count > rounds));
1311
1312         if (overlap_obj) {
1313                 for (i = 0; i < numcollobj; i++) {
1314                         MEM_SAFE_FREE(overlap_obj[i]);
1315                 }
1316
1317                 MEM_freeN(overlap_obj);
1318         }
1319
1320         MEM_SAFE_FREE(coll_counts_obj);
1321
1322         MEM_SAFE_FREE(overlap_self);
1323
1324         BKE_collision_objects_free(collobjs);
1325
1326         return MIN2(ret, 1);
1327 }
1328
1329 BLI_INLINE void max_v3_v3v3(float r[3], const float a[3], const float b[3])
1330 {
1331         r[0] = max_ff(a[0], b[0]);
1332         r[1] = max_ff(a[1], b[1]);
1333         r[2] = max_ff(a[2], b[2]);
1334 }
1335
1336 void collision_get_collider_velocity(float vel_old[3], float vel_new[3], CollisionModifierData *collmd, CollPair *collpair)
1337 {
1338         float u1, u2, u3;
1339
1340         /* compute barycentric coordinates */
1341         collision_compute_barycentric(collpair->pb,
1342                                       collmd->current_x[collpair->bp1].co,
1343                                       collmd->current_x[collpair->bp2].co,
1344                                       collmd->current_x[collpair->bp3].co,
1345                                       &u1, &u2, &u3);
1346
1347         collision_interpolateOnTriangle(vel_new, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3);
1348         /* XXX assume constant velocity of the collider for now */
1349         copy_v3_v3(vel_old, vel_new);
1350 }
1351
1352 BLI_INLINE bool cloth_point_face_collision_params(const float p1[3], const float p2[3], const float v0[3], const float v1[3], const float v2[3],
1353                                                   float r_nor[3], float *r_lambda, float r_w[3])
1354 {
1355         float edge1[3], edge2[3], p2face[3], p1p2[3], v0p2[3];
1356         float nor_v0p2, nor_p1p2;
1357
1358         sub_v3_v3v3(edge1, v1, v0);
1359         sub_v3_v3v3(edge2, v2, v0);
1360         cross_v3_v3v3(r_nor, edge1, edge2);
1361         normalize_v3(r_nor);
1362
1363         sub_v3_v3v3(v0p2, p2, v0);
1364         nor_v0p2 = dot_v3v3(v0p2, r_nor);
1365         madd_v3_v3v3fl(p2face, p2, r_nor, -nor_v0p2);
1366         interp_weights_tri_v3(r_w, v0, v1, v2, p2face);
1367
1368         sub_v3_v3v3(p1p2, p2, p1);
1369         nor_p1p2 = dot_v3v3(p1p2, r_nor);
1370         *r_lambda = (nor_p1p2 != 0.0f ? nor_v0p2 / nor_p1p2 : 0.0f);
1371
1372         return r_w[1] >= 0.0f && r_w[2] >= 0.0f && r_w[1] + r_w[2] <= 1.0f;
1373 }
1374
1375 static CollPair *cloth_point_collpair(
1376         float p1[3], float p2[3], const MVert *mverts, int bp1, int bp2, int bp3,
1377         int index_cloth, int index_coll, float epsilon, CollPair *collpair)
1378 {
1379         const float *co1 = mverts[bp1].co, *co2 = mverts[bp2].co, *co3 = mverts[bp3].co;
1380         float lambda /*, distance1 */, distance2;
1381         float facenor[3], v1p1[3], v1p2[3];
1382         float w[3];
1383
1384         if (!cloth_point_face_collision_params(p1, p2, co1, co2, co3, facenor, &lambda, w))
1385                 return collpair;
1386
1387         sub_v3_v3v3(v1p1, p1, co1);
1388 //      distance1 = dot_v3v3(v1p1, facenor);
1389         sub_v3_v3v3(v1p2, p2, co1);
1390         distance2 = dot_v3v3(v1p2, facenor);
1391 //      if (distance2 > epsilon || (distance1 < 0.0f && distance2 < 0.0f))
1392         if (distance2 > epsilon)
1393                 return collpair;
1394
1395         collpair->face1 = index_cloth; /* XXX actually not a face, but equivalent index for point */
1396         collpair->face2 = index_coll;
1397         collpair->ap1 = index_cloth;
1398         collpair->ap2 = collpair->ap3 = -1; /* unused */
1399         collpair->bp1 = bp1;
1400         collpair->bp2 = bp2;
1401         collpair->bp3 = bp3;
1402
1403         /* note: using the second point here, which is
1404          * the current updated position that needs to be corrected
1405          */
1406         copy_v3_v3(collpair->pa, p2);
1407         collpair->distance = distance2;
1408         mul_v3_v3fl(collpair->vector, facenor, -distance2);
1409
1410         interp_v3_v3v3v3(collpair->pb, co1, co2, co3, w);
1411
1412         copy_v3_v3(collpair->normal, facenor);
1413         collpair->time = lambda;
1414         collpair->flag = 0;
1415
1416         collpair++;
1417         return collpair;
1418 }
1419
1420 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
1421 static CollPair *cloth_point_collision(
1422         ModifierData *md1, ModifierData *md2,
1423         BVHTreeOverlap *overlap, float epsilon, CollPair *collpair, float UNUSED(dt))
1424 {
1425         ClothModifierData *clmd = (ClothModifierData *)md1;
1426         CollisionModifierData *collmd = (CollisionModifierData *) md2;
1427         /* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
1428         ClothVertex *vert = NULL;
1429         const MVertTri *vt;
1430         const MVert *mverts = collmd->current_x;
1431
1432         vert = &clmd->clothObject->verts[overlap->indexA];
1433         vt = &collmd->tri[overlap->indexB];
1434
1435         collpair = cloth_point_collpair(
1436                 vert->tx, vert->x, mverts,
1437                 vt->tri[0], vt->tri[1], vt->tri[2],
1438                 overlap->indexA, overlap->indexB,
1439                 epsilon, collpair);
1440
1441         return collpair;
1442 }
1443
1444 static void cloth_points_objcollisions_nearcheck(
1445         ClothModifierData *clmd, CollisionModifierData *collmd,
1446         CollPair **collisions, CollPair **collisions_index,
1447         int numresult, BVHTreeOverlap *overlap, float epsilon, double dt)
1448 {
1449         int i;
1450
1451         /* can return 2 collisions in total */
1452         *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 2, "collision array" );
1453         *collisions_index = *collisions;
1454
1455         for ( i = 0; i < numresult; i++ ) {
1456                 *collisions_index = cloth_point_collision((ModifierData *)clmd, (ModifierData *)collmd,
1457                                                           overlap+i, epsilon, *collisions_index, dt);
1458         }
1459 }
1460
1461 void cloth_find_point_contacts(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt,
1462                                ColliderContacts **r_collider_contacts, int *r_totcolliders)
1463 {
1464         Cloth *cloth= clmd->clothObject;
1465         BVHTree *cloth_bvh;
1466         unsigned int i = 0, mvert_num = 0;
1467         ClothVertex *verts = NULL;
1468
1469         ColliderContacts *collider_contacts;
1470
1471         Object **collobjs = NULL;
1472         unsigned int numcollobj = 0;
1473
1474         verts = cloth->verts;
1475         mvert_num = cloth->mvert_num;
1476
1477         ////////////////////////////////////////////////////////////
1478         // static collisions
1479         ////////////////////////////////////////////////////////////
1480
1481         /* Check we do have collision objects to test against, before doing anything else. */
1482         collobjs = BKE_collision_objects_create(depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
1483         if (!collobjs) {
1484                 *r_collider_contacts = NULL;
1485                 *r_totcolliders = 0;
1486                 return;
1487         }
1488
1489         // create temporary cloth points bvh
1490         cloth_bvh = BLI_bvhtree_new(mvert_num, clmd->coll_parms->epsilon, 4, 6);
1491         /* fill tree */
1492         for (i = 0; i < mvert_num; i++) {
1493                 float co[6];
1494
1495                 copy_v3_v3(&co[0*3], verts[i].x);
1496                 copy_v3_v3(&co[1*3], verts[i].tx);
1497
1498                 BLI_bvhtree_insert(cloth_bvh, i, co, 2);
1499         }
1500         /* balance tree */
1501         BLI_bvhtree_balance(cloth_bvh);
1502
1503         /* move object to position (step) in time */
1504         for (i = 0; i < numcollobj; i++) {
1505                 Object *collob= collobjs[i];
1506                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1507                 if (!collmd->bvhtree)
1508                         continue;
1509
1510                 /* move object to position (step) in time */
1511                 collision_move_object ( collmd, step + dt, step );
1512         }
1513
1514         collider_contacts = MEM_callocN(sizeof(ColliderContacts) * numcollobj, "CollPair");
1515
1516         // check all collision objects
1517         for (i = 0; i < numcollobj; i++) {
1518                 ColliderContacts *ct = collider_contacts + i;
1519                 Object *collob= collobjs[i];
1520                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1521                 BVHTreeOverlap *overlap;
1522                 unsigned int result = 0;
1523                 float epsilon;
1524
1525                 ct->ob = collob;
1526                 ct->collmd = collmd;
1527                 ct->collisions = NULL;
1528                 ct->totcollisions = 0;
1529
1530                 if (!collmd->bvhtree)
1531                         continue;
1532
1533                 /* search for overlapping collision pairs */
1534                 overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL);
1535                 epsilon = BLI_bvhtree_get_epsilon(collmd->bvhtree);
1536
1537                 // go to next object if no overlap is there
1538                 if (result && overlap) {
1539                         CollPair *collisions_index;
1540
1541                         /* check if collisions really happen (costly near check) */
1542                         cloth_points_objcollisions_nearcheck(clmd, collmd, &ct->collisions, &collisions_index,
1543                                                              result, overlap, epsilon, dt);
1544                         ct->totcollisions = (int)(collisions_index - ct->collisions);
1545
1546                         // resolve nearby collisions
1547 //                      ret += cloth_points_objcollisions_resolve(clmd, collmd, collob->pd, collisions[i], collisions_index[i], dt);
1548                 }
1549
1550                 if (overlap)
1551                         MEM_freeN(overlap);
1552         }
1553
1554         BKE_collision_objects_free(collobjs);
1555
1556         BLI_bvhtree_free(cloth_bvh);
1557
1558         ////////////////////////////////////////////////////////////
1559         // update positions
1560         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1561         ////////////////////////////////////////////////////////////
1562
1563         // verts come from clmd
1564         for (i = 0; i < mvert_num; i++) {
1565                 if (clmd->sim_parms->vgroup_mass > 0) {
1566                         if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
1567                                 continue;
1568                         }
1569                 }
1570
1571                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1572         }
1573         ////////////////////////////////////////////////////////////
1574
1575         *r_collider_contacts = collider_contacts;
1576         *r_totcolliders = numcollobj;
1577 }
1578
1579 void cloth_free_contacts(ColliderContacts *collider_contacts, int totcolliders)
1580 {
1581         if (collider_contacts) {
1582                 int i;
1583                 for (i = 0; i < totcolliders; ++i) {
1584                         ColliderContacts *ct = collider_contacts + i;
1585                         if (ct->collisions) {
1586                                 MEM_freeN(ct->collisions);
1587                         }
1588                 }
1589                 MEM_freeN(collider_contacts);
1590         }
1591 }