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