Fix T67459: Dope Editor, muting channels with shortcut doesn't work
[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
453   w2[0] = (float)((f - b * (double)w1[0]) / c);
454
455   if (w2[0] < 0) {
456     w2[0] = 0;
457   }
458
459   w3[0] = 1.0f - w1[0] - w2[0];
460
461 #undef INPR
462 }
463
464 #ifdef __GNUC__
465 #  pragma GCC diagnostic push
466 #  pragma GCC diagnostic ignored "-Wdouble-promotion"
467 #endif
468
469 DO_INLINE void collision_interpolateOnTriangle(
470     float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3)
471 {
472   zero_v3(to);
473   VECADDMUL(to, v1, w1);
474   VECADDMUL(to, v2, w2);
475   VECADDMUL(to, v3, w3);
476 }
477
478 static int cloth_collision_response_static(ClothModifierData *clmd,
479                                            CollisionModifierData *collmd,
480                                            Object *collob,
481                                            CollPair *collpair,
482                                            uint collision_count,
483                                            const float dt)
484 {
485   int result = 0;
486   Cloth *cloth1;
487   float w1, w2, w3, u1, u2, u3;
488   float v1[3], v2[3], relativeVelocity[3];
489   float magrelVel;
490   float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
491
492   cloth1 = clmd->clothObject;
493
494   for (int i = 0; i < collision_count; i++, collpair++) {
495     float i1[3], i2[3], i3[3];
496
497     zero_v3(i1);
498     zero_v3(i2);
499     zero_v3(i3);
500
501     /* Only handle static collisions here. */
502     if (collpair->flag & (COLLISION_IN_FUTURE | COLLISION_INACTIVE)) {
503       continue;
504     }
505
506     /* Compute barycentric coordinates for both collision points. */
507     collision_compute_barycentric(collpair->pa,
508                                   cloth1->verts[collpair->ap1].tx,
509                                   cloth1->verts[collpair->ap2].tx,
510                                   cloth1->verts[collpair->ap3].tx,
511                                   &w1,
512                                   &w2,
513                                   &w3);
514
515     collision_compute_barycentric(collpair->pb,
516                                   collmd->current_x[collpair->bp1].co,
517                                   collmd->current_x[collpair->bp2].co,
518                                   collmd->current_x[collpair->bp3].co,
519                                   &u1,
520                                   &u2,
521                                   &u3);
522
523     /* Calculate relative "velocity". */
524     collision_interpolateOnTriangle(v1,
525                                     cloth1->verts[collpair->ap1].tv,
526                                     cloth1->verts[collpair->ap2].tv,
527                                     cloth1->verts[collpair->ap3].tv,
528                                     w1,
529                                     w2,
530                                     w3);
531
532     collision_interpolateOnTriangle(v2,
533                                     collmd->current_v[collpair->bp1].co,
534                                     collmd->current_v[collpair->bp2].co,
535                                     collmd->current_v[collpair->bp3].co,
536                                     u1,
537                                     u2,
538                                     u3);
539
540     sub_v3_v3v3(relativeVelocity, v2, v1);
541
542     /* Calculate the normal component of the relative velocity
543      * (actually only the magnitude - the direction is stored in 'normal'). */
544     magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
545
546     /* If magrelVel < 0 the edges are approaching each other. */
547     if (magrelVel > 0.0f) {
548       /* Calculate Impulse magnitude to stop all motion in normal direction. */
549       float magtangent = 0, repulse = 0, d = 0;
550       double impulse = 0.0;
551       float vrel_t_pre[3];
552       float temp[3];
553       float time_multiplier;
554
555       /* Calculate tangential velocity. */
556       copy_v3_v3(temp, collpair->normal);
557       mul_v3_fl(temp, magrelVel);
558       sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
559
560       /* Decrease in magnitude of relative tangential velocity due to coulomb friction
561        * in original formula "magrelVel" should be the
562        * "change of relative velocity in normal direction". */
563       magtangent = min_ff(collob->pd->pdef_cfrict * 0.01f * magrelVel, len_v3(vrel_t_pre));
564
565       /* Apply friction impulse. */
566       if (magtangent > ALMOST_ZERO) {
567         normalize_v3(vrel_t_pre);
568
569         impulse = magtangent / 1.5;
570
571         VECADDMUL(i1, vrel_t_pre, w1 * impulse);
572         VECADDMUL(i2, vrel_t_pre, w2 * impulse);
573         VECADDMUL(i3, vrel_t_pre, w3 * impulse);
574       }
575
576       /* Apply velocity stopping impulse. */
577       impulse = magrelVel / 1.5f;
578
579       VECADDMUL(i1, collpair->normal, w1 * impulse);
580       cloth1->verts[collpair->ap1].impulse_count++;
581
582       VECADDMUL(i2, collpair->normal, w2 * impulse);
583       cloth1->verts[collpair->ap2].impulse_count++;
584
585       VECADDMUL(i3, collpair->normal, w3 * impulse);
586       cloth1->verts[collpair->ap3].impulse_count++;
587
588       time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
589
590       d = clmd->coll_parms->epsilon * 8.0f / 9.0f + epsilon2 * 8.0f / 9.0f - collpair->distance;
591
592       if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) {
593         repulse = MIN2(d / time_multiplier, 0.1f * d * time_multiplier - magrelVel);
594
595         /* Stay on the safe side and clamp repulse. */
596         if (impulse > ALMOST_ZERO) {
597           repulse = min_ff(repulse, 5.0f * impulse);
598         }
599
600         repulse = max_ff(impulse, repulse);
601
602         impulse = repulse / 1.5f;
603
604         VECADDMUL(i1, collpair->normal, impulse);
605         VECADDMUL(i2, collpair->normal, impulse);
606         VECADDMUL(i3, collpair->normal, impulse);
607       }
608
609       result = 1;
610     }
611     else {
612       float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
613       float d;
614
615       d = clmd->coll_parms->epsilon * 8.0f / 9.0f + epsilon2 * 8.0f / 9.0f - collpair->distance;
616
617       if (d > ALMOST_ZERO) {
618         /* Stay on the safe side and clamp repulse. */
619         float repulse = d / time_multiplier;
620         float impulse = repulse / 4.5f;
621
622         VECADDMUL(i1, collpair->normal, w1 * impulse);
623         VECADDMUL(i2, collpair->normal, w2 * impulse);
624         VECADDMUL(i3, collpair->normal, w3 * impulse);
625
626         cloth1->verts[collpair->ap1].impulse_count++;
627         cloth1->verts[collpair->ap2].impulse_count++;
628         cloth1->verts[collpair->ap3].impulse_count++;
629
630         result = 1;
631       }
632     }
633
634     if (result) {
635       float clamp = clmd->coll_parms->clamp * dt;
636
637       if ((clamp > 0.0f) &&
638           ((len_v3(i1) > clamp) || (len_v3(i2) > clamp) || (len_v3(i3) > clamp))) {
639         return 0;
640       }
641
642       for (int j = 0; j < 3; j++) {
643         if (cloth1->verts[collpair->ap1].impulse_count > 0 &&
644             ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j])) {
645           cloth1->verts[collpair->ap1].impulse[j] = i1[j];
646         }
647
648         if (cloth1->verts[collpair->ap2].impulse_count > 0 &&
649             ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j])) {
650           cloth1->verts[collpair->ap2].impulse[j] = i2[j];
651         }
652
653         if (cloth1->verts[collpair->ap3].impulse_count > 0 &&
654             ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j])) {
655           cloth1->verts[collpair->ap3].impulse[j] = i3[j];
656         }
657       }
658     }
659   }
660
661   return result;
662 }
663
664 static int cloth_selfcollision_response_static(ClothModifierData *clmd,
665                                                CollPair *collpair,
666                                                uint collision_count,
667                                                const float dt)
668 {
669   int result = 0;
670   Cloth *cloth1;
671   float w1, w2, w3, u1, u2, u3;
672   float v1[3], v2[3], relativeVelocity[3];
673   float magrelVel;
674
675   cloth1 = clmd->clothObject;
676
677   for (int i = 0; i < collision_count; i++, collpair++) {
678     float i1[3], i2[3], i3[3];
679
680     zero_v3(i1);
681     zero_v3(i2);
682     zero_v3(i3);
683
684     /* Only handle static collisions here. */
685     if (collpair->flag & (COLLISION_IN_FUTURE | COLLISION_INACTIVE)) {
686       continue;
687     }
688
689     /* Compute barycentric coordinates for both collision points. */
690     collision_compute_barycentric(collpair->pa,
691                                   cloth1->verts[collpair->ap1].tx,
692                                   cloth1->verts[collpair->ap2].tx,
693                                   cloth1->verts[collpair->ap3].tx,
694                                   &w1,
695                                   &w2,
696                                   &w3);
697
698     collision_compute_barycentric(collpair->pb,
699                                   cloth1->verts[collpair->bp1].tx,
700                                   cloth1->verts[collpair->bp2].tx,
701                                   cloth1->verts[collpair->bp3].tx,
702                                   &u1,
703                                   &u2,
704                                   &u3);
705
706     /* Calculate relative "velocity". */
707     collision_interpolateOnTriangle(v1,
708                                     cloth1->verts[collpair->ap1].tv,
709                                     cloth1->verts[collpair->ap2].tv,
710                                     cloth1->verts[collpair->ap3].tv,
711                                     w1,
712                                     w2,
713                                     w3);
714
715     collision_interpolateOnTriangle(v2,
716                                     cloth1->verts[collpair->bp1].tv,
717                                     cloth1->verts[collpair->bp2].tv,
718                                     cloth1->verts[collpair->bp3].tv,
719                                     u1,
720                                     u2,
721                                     u3);
722
723     sub_v3_v3v3(relativeVelocity, v2, v1);
724
725     /* Calculate the normal component of the relative velocity
726      * (actually only the magnitude - the direction is stored in 'normal'). */
727     magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
728
729     /* TODO: Impulses should be weighed by mass as this is self col,
730      * this has to be done after mass distribution is implemented. */
731
732     /* If magrelVel < 0 the edges are approaching each other. */
733     if (magrelVel > 0.0f) {
734       /* Calculate Impulse magnitude to stop all motion in normal direction. */
735       float magtangent = 0, repulse = 0, d = 0;
736       double impulse = 0.0;
737       float vrel_t_pre[3];
738       float temp[3], time_multiplier;
739
740       /* Calculate tangential velocity. */
741       copy_v3_v3(temp, collpair->normal);
742       mul_v3_fl(temp, magrelVel);
743       sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
744
745       /* Decrease in magnitude of relative tangential velocity due to coulomb friction
746        * in original formula "magrelVel" should be the
747        * "change of relative velocity in normal direction". */
748       magtangent = min_ff(clmd->coll_parms->self_friction * 0.01f * magrelVel, len_v3(vrel_t_pre));
749
750       /* Apply friction impulse. */
751       if (magtangent > ALMOST_ZERO) {
752         normalize_v3(vrel_t_pre);
753
754         impulse = magtangent / 1.5;
755
756         VECADDMUL(i1, vrel_t_pre, w1 * impulse);
757         VECADDMUL(i2, vrel_t_pre, w2 * impulse);
758         VECADDMUL(i3, vrel_t_pre, w3 * impulse);
759       }
760
761       /* Apply velocity stopping impulse. */
762       impulse = magrelVel / 3.0f;
763
764       VECADDMUL(i1, collpair->normal, w1 * impulse);
765       cloth1->verts[collpair->ap1].impulse_count++;
766
767       VECADDMUL(i2, collpair->normal, w2 * impulse);
768       cloth1->verts[collpair->ap2].impulse_count++;
769
770       VECADDMUL(i3, collpair->normal, w3 * impulse);
771       cloth1->verts[collpair->ap3].impulse_count++;
772
773       time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
774
775       d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance;
776
777       if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) {
778         repulse = MIN2(d / time_multiplier, 0.1f * d * time_multiplier - magrelVel);
779
780         if (impulse > ALMOST_ZERO) {
781           repulse = min_ff(repulse, 5.0 * impulse);
782         }
783
784         repulse = max_ff(impulse, repulse);
785
786         impulse = repulse / 1.5f;
787
788         VECADDMUL(i1, collpair->normal, w1 * impulse);
789         VECADDMUL(i2, collpair->normal, w2 * impulse);
790         VECADDMUL(i3, collpair->normal, w3 * impulse);
791       }
792
793       result = 1;
794     }
795     else {
796       float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
797       float d;
798
799       d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance;
800
801       if (d > ALMOST_ZERO) {
802         /* Stay on the safe side and clamp repulse. */
803         float repulse = d * 1.0f / time_multiplier;
804         float impulse = repulse / 9.0f;
805
806         VECADDMUL(i1, collpair->normal, w1 * impulse);
807         VECADDMUL(i2, collpair->normal, w2 * impulse);
808         VECADDMUL(i3, collpair->normal, w3 * impulse);
809
810         cloth1->verts[collpair->ap1].impulse_count++;
811         cloth1->verts[collpair->ap2].impulse_count++;
812         cloth1->verts[collpair->ap3].impulse_count++;
813
814         result = 1;
815       }
816     }
817
818     if (result) {
819       float clamp = clmd->coll_parms->self_clamp * dt;
820
821       if ((clamp > 0.0f) &&
822           ((len_v3(i1) > clamp) || (len_v3(i2) > clamp) || (len_v3(i3) > clamp))) {
823         return 0;
824       }
825
826       for (int j = 0; j < 3; j++) {
827         if (cloth1->verts[collpair->ap1].impulse_count > 0 &&
828             ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j])) {
829           cloth1->verts[collpair->ap1].impulse[j] = i1[j];
830         }
831
832         if (cloth1->verts[collpair->ap2].impulse_count > 0 &&
833             ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j])) {
834           cloth1->verts[collpair->ap2].impulse[j] = i2[j];
835         }
836
837         if (cloth1->verts[collpair->ap3].impulse_count > 0 &&
838             ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j])) {
839           cloth1->verts[collpair->ap3].impulse[j] = i3[j];
840         }
841       }
842     }
843   }
844
845   return result;
846 }
847
848 #ifdef __GNUC__
849 #  pragma GCC diagnostic pop
850 #endif
851
852 static void cloth_collision(void *__restrict userdata,
853                             const int index,
854                             const ParallelRangeTLS *__restrict UNUSED(tls))
855 {
856   ColDetectData *data = (ColDetectData *)userdata;
857
858   ClothModifierData *clmd = data->clmd;
859   CollisionModifierData *collmd = data->collmd;
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 epsilon1 = clmd->coll_parms->epsilon;
865   float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
866   float pa[3], pb[3], vect[3];
867
868   tri_a = &clmd->clothObject->tri[data->overlap[index].indexA];
869   tri_b = &collmd->tri[data->overlap[index].indexB];
870
871   /* Compute distance and normal. */
872   distance = compute_collision_point(verts1[tri_a->tri[0]].tx,
873                                      verts1[tri_a->tri[1]].tx,
874                                      verts1[tri_a->tri[2]].tx,
875                                      collmd->current_x[tri_b->tri[0]].co,
876                                      collmd->current_x[tri_b->tri[1]].co,
877                                      collmd->current_x[tri_b->tri[2]].co,
878                                      data->culling,
879                                      data->use_normal,
880                                      pa,
881                                      pb,
882                                      vect);
883
884   if ((distance <= (epsilon1 + epsilon2 + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) {
885     collpair[index].ap1 = tri_a->tri[0];
886     collpair[index].ap2 = tri_a->tri[1];
887     collpair[index].ap3 = tri_a->tri[2];
888
889     collpair[index].bp1 = tri_b->tri[0];
890     collpair[index].bp2 = tri_b->tri[1];
891     collpair[index].bp3 = tri_b->tri[2];
892
893     copy_v3_v3(collpair[index].pa, pa);
894     copy_v3_v3(collpair[index].pb, pb);
895     copy_v3_v3(collpair[index].vector, vect);
896
897     normalize_v3_v3(collpair[index].normal, collpair[index].vector);
898
899     collpair[index].distance = distance;
900     collpair[index].flag = 0;
901
902     data->collided = true;
903   }
904   else {
905     collpair[index].flag = COLLISION_INACTIVE;
906   }
907 }
908
909 static void cloth_selfcollision(void *__restrict userdata,
910                                 const int index,
911                                 const ParallelRangeTLS *__restrict UNUSED(tls))
912 {
913   SelfColDetectData *data = (SelfColDetectData *)userdata;
914
915   ClothModifierData *clmd = data->clmd;
916   CollPair *collpair = data->collisions;
917   const MVertTri *tri_a, *tri_b;
918   ClothVertex *verts1 = clmd->clothObject->verts;
919   float distance = 0.0f;
920   float epsilon = clmd->coll_parms->selfepsilon;
921   float pa[3], pb[3], vect[3];
922
923   tri_a = &clmd->clothObject->tri[data->overlap[index].indexA];
924   tri_b = &clmd->clothObject->tri[data->overlap[index].indexB];
925
926   for (uint i = 0; i < 3; i++) {
927     for (uint j = 0; j < 3; j++) {
928       if (tri_a->tri[i] == tri_b->tri[j]) {
929         collpair[index].flag = COLLISION_INACTIVE;
930         return;
931       }
932     }
933   }
934
935   if (((verts1[tri_a->tri[0]].flags & verts1[tri_a->tri[1]].flags & verts1[tri_a->tri[2]].flags) |
936        (verts1[tri_b->tri[0]].flags & verts1[tri_b->tri[1]].flags & verts1[tri_b->tri[2]].flags)) &
937       CLOTH_VERT_FLAG_NOSELFCOLL) {
938     collpair[index].flag = COLLISION_INACTIVE;
939     return;
940   }
941
942   /* Compute distance and normal. */
943   distance = compute_collision_point(verts1[tri_a->tri[0]].tx,
944                                      verts1[tri_a->tri[1]].tx,
945                                      verts1[tri_a->tri[2]].tx,
946                                      verts1[tri_b->tri[0]].tx,
947                                      verts1[tri_b->tri[1]].tx,
948                                      verts1[tri_b->tri[2]].tx,
949                                      false,
950                                      false,
951                                      pa,
952                                      pb,
953                                      vect);
954
955   if ((distance <= (epsilon * 2.0f + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) {
956     collpair[index].ap1 = tri_a->tri[0];
957     collpair[index].ap2 = tri_a->tri[1];
958     collpair[index].ap3 = tri_a->tri[2];
959
960     collpair[index].bp1 = tri_b->tri[0];
961     collpair[index].bp2 = tri_b->tri[1];
962     collpair[index].bp3 = tri_b->tri[2];
963
964     copy_v3_v3(collpair[index].pa, pa);
965     copy_v3_v3(collpair[index].pb, pb);
966     copy_v3_v3(collpair[index].vector, vect);
967
968     normalize_v3_v3(collpair[index].normal, collpair[index].vector);
969
970     collpair[index].distance = distance;
971     collpair[index].flag = 0;
972
973     data->collided = true;
974   }
975   else {
976     collpair[index].flag = COLLISION_INACTIVE;
977   }
978 }
979
980 static void add_collision_object(ListBase *relations,
981                                  Object *ob,
982                                  int level,
983                                  unsigned int modifier_type)
984 {
985   CollisionModifierData *cmd = NULL;
986
987   /* only get objects with collision modifier */
988   if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) ||
989       (modifier_type != eModifierType_Collision)) {
990     cmd = (CollisionModifierData *)modifiers_findByType(ob, modifier_type);
991   }
992
993   if (cmd) {
994     CollisionRelation *relation = MEM_callocN(sizeof(CollisionRelation), "CollisionRelation");
995     relation->ob = ob;
996     BLI_addtail(relations, relation);
997   }
998
999   /* objects in dupli groups, one level only for now */
1000   /* TODO: this doesn't really work, we are not taking into account the
1001    * dupli transforms and can get objects in the list multiple times. */
1002   if (ob->instance_collection && level == 0) {
1003     Collection *collection = ob->instance_collection;
1004
1005     /* add objects */
1006     FOREACH_COLLECTION_OBJECT_RECURSIVE_BEGIN (collection, object) {
1007       add_collision_object(relations, object, level + 1, modifier_type);
1008     }
1009     FOREACH_COLLECTION_OBJECT_RECURSIVE_END;
1010   }
1011 }
1012
1013 /* Create list of collision relations in the collection or entire scene.
1014  * This is used by the depsgraph to build relations, as well as faster
1015  * lookup of colliders during evaluation. */
1016 ListBase *BKE_collision_relations_create(Depsgraph *depsgraph,
1017                                          Collection *collection,
1018                                          unsigned int modifier_type)
1019 {
1020   ViewLayer *view_layer = DEG_get_input_view_layer(depsgraph);
1021   Base *base = BKE_collection_or_layer_objects(view_layer, collection);
1022   const bool for_render = (DEG_get_mode(depsgraph) == DAG_EVAL_RENDER);
1023   const int base_flag = (for_render) ? BASE_ENABLED_RENDER : BASE_ENABLED_VIEWPORT;
1024
1025   ListBase *relations = MEM_callocN(sizeof(ListBase), "CollisionRelation list");
1026
1027   for (; base; base = base->next) {
1028     if (base->flag & base_flag) {
1029       add_collision_object(relations, base->object, 0, modifier_type);
1030     }
1031   }
1032
1033   return relations;
1034 }
1035
1036 void BKE_collision_relations_free(ListBase *relations)
1037 {
1038   if (relations) {
1039     BLI_freelistN(relations);
1040     MEM_freeN(relations);
1041   }
1042 }
1043
1044 /* Create effective list of colliders from relations built beforehand.
1045  * Self will be excluded. */
1046 Object **BKE_collision_objects_create(Depsgraph *depsgraph,
1047                                       Object *self,
1048                                       Collection *collection,
1049                                       unsigned int *numcollobj,
1050                                       unsigned int modifier_type)
1051 {
1052   ListBase *relations = DEG_get_collision_relations(depsgraph, collection, modifier_type);
1053
1054   if (!relations) {
1055     *numcollobj = 0;
1056     return NULL;
1057   }
1058
1059   int maxnum = BLI_listbase_count(relations);
1060   int num = 0;
1061   Object **objects = MEM_callocN(sizeof(Object *) * maxnum, __func__);
1062
1063   for (CollisionRelation *relation = relations->first; relation; relation = relation->next) {
1064     /* Get evaluated object. */
1065     Object *ob = (Object *)DEG_get_evaluated_id(depsgraph, &relation->ob->id);
1066
1067     if (ob != self) {
1068       objects[num] = ob;
1069       num++;
1070     }
1071   }
1072
1073   if (num == 0) {
1074     MEM_freeN(objects);
1075     objects = NULL;
1076   }
1077
1078   *numcollobj = num;
1079   return objects;
1080 }
1081
1082 void BKE_collision_objects_free(Object **objects)
1083 {
1084   if (objects) {
1085     MEM_freeN(objects);
1086   }
1087 }
1088
1089 /* Create effective list of colliders from relations built beforehand.
1090  * Self will be excluded. */
1091 ListBase *BKE_collider_cache_create(Depsgraph *depsgraph, Object *self, Collection *collection)
1092 {
1093   ListBase *relations = DEG_get_collision_relations(
1094       depsgraph, collection, eModifierType_Collision);
1095   ListBase *cache = NULL;
1096
1097   if (!relations) {
1098     return NULL;
1099   }
1100
1101   for (CollisionRelation *relation = relations->first; relation; relation = relation->next) {
1102     /* Get evaluated object. */
1103     Object *ob = (Object *)DEG_get_evaluated_id(depsgraph, &relation->ob->id);
1104
1105     if (ob == self) {
1106       continue;
1107     }
1108
1109     CollisionModifierData *cmd = (CollisionModifierData *)modifiers_findByType(
1110         ob, eModifierType_Collision);
1111     if (cmd && cmd->bvhtree) {
1112       if (cache == NULL) {
1113         cache = MEM_callocN(sizeof(ListBase), "ColliderCache array");
1114       }
1115
1116       ColliderCache *col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
1117       col->ob = ob;
1118       col->collmd = cmd;
1119       /* make sure collider is properly set up */
1120       collision_move_object(cmd, 1.0, 0.0);
1121       BLI_addtail(cache, col);
1122     }
1123   }
1124
1125   return cache;
1126 }
1127
1128 void BKE_collider_cache_free(ListBase **colliders)
1129 {
1130   if (*colliders) {
1131     BLI_freelistN(*colliders);
1132     MEM_freeN(*colliders);
1133     *colliders = NULL;
1134   }
1135 }
1136
1137 static bool cloth_bvh_objcollisions_nearcheck(ClothModifierData *clmd,
1138                                               CollisionModifierData *collmd,
1139                                               CollPair **collisions,
1140                                               int numresult,
1141                                               BVHTreeOverlap *overlap,
1142                                               bool culling,
1143                                               bool use_normal)
1144 {
1145   *collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * numresult, "collision array");
1146
1147   ColDetectData data = {
1148       .clmd = clmd,
1149       .collmd = collmd,
1150       .overlap = overlap,
1151       .collisions = *collisions,
1152       .culling = culling,
1153       .use_normal = use_normal,
1154       .collided = false,
1155   };
1156
1157   ParallelRangeSettings settings;
1158   BLI_parallel_range_settings_defaults(&settings);
1159   settings.use_threading = true;
1160   BLI_task_parallel_range(0, numresult, &data, cloth_collision, &settings);
1161
1162   return data.collided;
1163 }
1164
1165 static bool cloth_bvh_selfcollisions_nearcheck(ClothModifierData *clmd,
1166                                                CollPair *collisions,
1167                                                int numresult,
1168                                                BVHTreeOverlap *overlap)
1169 {
1170   SelfColDetectData data = {
1171       .clmd = clmd,
1172       .overlap = overlap,
1173       .collisions = collisions,
1174       .collided = false,
1175   };
1176
1177   ParallelRangeSettings settings;
1178   BLI_parallel_range_settings_defaults(&settings);
1179   settings.use_threading = true;
1180   BLI_task_parallel_range(0, numresult, &data, cloth_selfcollision, &settings);
1181
1182   return data.collided;
1183 }
1184
1185 static int cloth_bvh_objcollisions_resolve(ClothModifierData *clmd,
1186                                            Object **collobjs,
1187                                            CollPair **collisions,
1188                                            uint *collision_counts,
1189                                            const uint numcollobj,
1190                                            const float dt)
1191 {
1192   Cloth *cloth = clmd->clothObject;
1193   int i = 0, j = 0, mvert_num = 0;
1194   ClothVertex *verts = NULL;
1195   int ret = 0;
1196   int result = 0;
1197
1198   mvert_num = clmd->clothObject->mvert_num;
1199   verts = cloth->verts;
1200
1201   result = 1;
1202
1203   for (j = 0; j < 2; j++) {
1204     result = 0;
1205
1206     for (i = 0; i < numcollobj; i++) {
1207       Object *collob = collobjs[i];
1208       CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(
1209           collob, eModifierType_Collision);
1210
1211       if (collmd->bvhtree) {
1212         result += cloth_collision_response_static(
1213             clmd, collmd, collob, collisions[i], collision_counts[i], dt);
1214       }
1215     }
1216
1217     /* Apply impulses in parallel. */
1218     if (result) {
1219       for (i = 0; i < mvert_num; i++) {
1220         // calculate "velocities" (just xnew = xold + v; no dt in v)
1221         if (verts[i].impulse_count) {
1222           add_v3_v3(verts[i].tv, verts[i].impulse);
1223           add_v3_v3(verts[i].dcvel, verts[i].impulse);
1224           zero_v3(verts[i].impulse);
1225           verts[i].impulse_count = 0;
1226
1227           ret++;
1228         }
1229       }
1230     }
1231     else {
1232       break;
1233     }
1234   }
1235   return ret;
1236 }
1237
1238 static int cloth_bvh_selfcollisions_resolve(ClothModifierData *clmd,
1239                                             CollPair *collisions,
1240                                             int collision_count,
1241                                             const float dt)
1242 {
1243   Cloth *cloth = clmd->clothObject;
1244   int i = 0, j = 0, mvert_num = 0;
1245   ClothVertex *verts = NULL;
1246   int ret = 0;
1247   int result = 0;
1248
1249   mvert_num = clmd->clothObject->mvert_num;
1250   verts = cloth->verts;
1251
1252   for (j = 0; j < 2; j++) {
1253     result = 0;
1254
1255     result += cloth_selfcollision_response_static(clmd, collisions, collision_count, dt);
1256
1257     /* Apply impulses in parallel. */
1258     if (result) {
1259       for (i = 0; i < mvert_num; i++) {
1260         if (verts[i].impulse_count) {
1261           // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1262           add_v3_v3(verts[i].tv, verts[i].impulse);
1263           add_v3_v3(verts[i].dcvel, verts[i].impulse);
1264           zero_v3(verts[i].impulse);
1265           verts[i].impulse_count = 0;
1266
1267           ret++;
1268         }
1269       }
1270     }
1271
1272     if (!result) {
1273       break;
1274     }
1275   }
1276   return ret;
1277 }
1278
1279 int cloth_bvh_collision(
1280     Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt)
1281 {
1282   Cloth *cloth = clmd->clothObject;
1283   BVHTree *cloth_bvh = cloth->bvhtree;
1284   uint i = 0, mvert_num = 0;
1285   int rounds = 0;
1286   ClothVertex *verts = NULL;
1287   int ret = 0, ret2 = 0;
1288   Object **collobjs = NULL;
1289   unsigned int numcollobj = 0;
1290   uint *coll_counts_obj = NULL;
1291   BVHTreeOverlap **overlap_obj = NULL;
1292   uint coll_count_self = 0;
1293   BVHTreeOverlap *overlap_self = NULL;
1294
1295   if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh == NULL) {
1296     return 0;
1297   }
1298
1299   verts = cloth->verts;
1300   mvert_num = cloth->mvert_num;
1301
1302   if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
1303     bvhtree_update_from_cloth(clmd, false, false);
1304
1305     collobjs = BKE_collision_objects_create(
1306         depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
1307
1308     if (collobjs) {
1309       coll_counts_obj = MEM_callocN(sizeof(uint) * numcollobj, "CollCounts");
1310       overlap_obj = MEM_callocN(sizeof(*overlap_obj) * numcollobj, "BVHOverlap");
1311
1312       for (i = 0; i < numcollobj; i++) {
1313         Object *collob = collobjs[i];
1314         CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(
1315             collob, eModifierType_Collision);
1316
1317         if (!collmd->bvhtree) {
1318           continue;
1319         }
1320
1321         /* Move object to position (step) in time. */
1322         collision_move_object(collmd, step + dt, step);
1323
1324         overlap_obj[i] = BLI_bvhtree_overlap(
1325             cloth_bvh, collmd->bvhtree, &coll_counts_obj[i], NULL, NULL);
1326       }
1327     }
1328   }
1329
1330   if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) {
1331     bvhtree_update_from_cloth(clmd, false, true);
1332
1333     overlap_self = BLI_bvhtree_overlap(
1334         cloth->bvhselftree, cloth->bvhselftree, &coll_count_self, NULL, NULL);
1335   }
1336
1337   do {
1338     ret2 = 0;
1339
1340     /* Object collisions. */
1341     if ((clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) && collobjs) {
1342       CollPair **collisions;
1343       bool collided = false;
1344
1345       collisions = MEM_callocN(sizeof(CollPair *) * numcollobj, "CollPair");
1346
1347       for (i = 0; i < numcollobj; i++) {
1348         Object *collob = collobjs[i];
1349         CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(
1350             collob, eModifierType_Collision);
1351
1352         if (!collmd->bvhtree) {
1353           continue;
1354         }
1355
1356         if (coll_counts_obj[i] && overlap_obj[i]) {
1357           collided = cloth_bvh_objcollisions_nearcheck(
1358                          clmd,
1359                          collmd,
1360                          &collisions[i],
1361                          coll_counts_obj[i],
1362                          overlap_obj[i],
1363                          (collob->pd->flag & PFIELD_CLOTH_USE_CULLING),
1364                          (collob->pd->flag & PFIELD_CLOTH_USE_NORMAL)) ||
1365                      collided;
1366         }
1367       }
1368
1369       if (collided) {
1370         ret += cloth_bvh_objcollisions_resolve(
1371             clmd, collobjs, collisions, coll_counts_obj, numcollobj, dt);
1372         ret2 += ret;
1373       }
1374
1375       for (i = 0; i < numcollobj; i++) {
1376         MEM_SAFE_FREE(collisions[i]);
1377       }
1378
1379       MEM_freeN(collisions);
1380     }
1381
1382     /* Self collisions. */
1383     if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) {
1384       CollPair *collisions = NULL;
1385
1386       verts = cloth->verts;
1387       mvert_num = cloth->mvert_num;
1388
1389       if (cloth->bvhselftree) {
1390         if (coll_count_self && overlap_self) {
1391           collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * coll_count_self,
1392                                                "collision array");
1393
1394           if (cloth_bvh_selfcollisions_nearcheck(
1395                   clmd, collisions, coll_count_self, overlap_self)) {
1396             ret += cloth_bvh_selfcollisions_resolve(clmd, collisions, coll_count_self, dt);
1397             ret2 += ret;
1398           }
1399         }
1400       }
1401
1402       MEM_SAFE_FREE(collisions);
1403     }
1404
1405     /* Apply all collision resolution. */
1406     if (ret2) {
1407       for (i = 0; i < mvert_num; i++) {
1408         if (clmd->sim_parms->vgroup_mass > 0) {
1409           if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
1410             continue;
1411           }
1412         }
1413
1414         add_v3_v3v3(verts[i].tx, verts[i].txold, verts[i].tv);
1415       }
1416     }
1417
1418     rounds++;
1419   } while (ret2 && (clmd->coll_parms->loop_count > rounds));
1420
1421   if (overlap_obj) {
1422     for (i = 0; i < numcollobj; i++) {
1423       MEM_SAFE_FREE(overlap_obj[i]);
1424     }
1425
1426     MEM_freeN(overlap_obj);
1427   }
1428
1429   MEM_SAFE_FREE(coll_counts_obj);
1430
1431   MEM_SAFE_FREE(overlap_self);
1432
1433   BKE_collision_objects_free(collobjs);
1434
1435   return MIN2(ret, 1);
1436 }
1437
1438 BLI_INLINE void max_v3_v3v3(float r[3], const float a[3], const float b[3])
1439 {
1440   r[0] = max_ff(a[0], b[0]);
1441   r[1] = max_ff(a[1], b[1]);
1442   r[2] = max_ff(a[2], b[2]);
1443 }
1444
1445 void collision_get_collider_velocity(float vel_old[3],
1446                                      float vel_new[3],
1447                                      CollisionModifierData *collmd,
1448                                      CollPair *collpair)
1449 {
1450   float u1, u2, u3;
1451
1452   /* compute barycentric coordinates */
1453   collision_compute_barycentric(collpair->pb,
1454                                 collmd->current_x[collpair->bp1].co,
1455                                 collmd->current_x[collpair->bp2].co,
1456                                 collmd->current_x[collpair->bp3].co,
1457                                 &u1,
1458                                 &u2,
1459                                 &u3);
1460
1461   collision_interpolateOnTriangle(vel_new,
1462                                   collmd->current_v[collpair->bp1].co,
1463                                   collmd->current_v[collpair->bp2].co,
1464                                   collmd->current_v[collpair->bp3].co,
1465                                   u1,
1466                                   u2,
1467                                   u3);
1468   /* XXX assume constant velocity of the collider for now */
1469   copy_v3_v3(vel_old, vel_new);
1470 }
1471
1472 BLI_INLINE bool cloth_point_face_collision_params(const float p1[3],
1473                                                   const float p2[3],
1474                                                   const float v0[3],
1475                                                   const float v1[3],
1476                                                   const float v2[3],
1477                                                   float r_nor[3],
1478                                                   float *r_lambda,
1479                                                   float r_w[3])
1480 {
1481   float edge1[3], edge2[3], p2face[3], p1p2[3], v0p2[3];
1482   float nor_v0p2, nor_p1p2;
1483
1484   sub_v3_v3v3(edge1, v1, v0);
1485   sub_v3_v3v3(edge2, v2, v0);
1486   cross_v3_v3v3(r_nor, edge1, edge2);
1487   normalize_v3(r_nor);
1488
1489   sub_v3_v3v3(v0p2, p2, v0);
1490   nor_v0p2 = dot_v3v3(v0p2, r_nor);
1491   madd_v3_v3v3fl(p2face, p2, r_nor, -nor_v0p2);
1492   interp_weights_tri_v3(r_w, v0, v1, v2, p2face);
1493
1494   sub_v3_v3v3(p1p2, p2, p1);
1495   nor_p1p2 = dot_v3v3(p1p2, r_nor);
1496   *r_lambda = (nor_p1p2 != 0.0f ? nor_v0p2 / nor_p1p2 : 0.0f);
1497
1498   return r_w[1] >= 0.0f && r_w[2] >= 0.0f && r_w[1] + r_w[2] <= 1.0f;
1499 }
1500
1501 static CollPair *cloth_point_collpair(float p1[3],
1502                                       float p2[3],
1503                                       const MVert *mverts,
1504                                       int bp1,
1505                                       int bp2,
1506                                       int bp3,
1507                                       int index_cloth,
1508                                       int index_coll,
1509                                       float epsilon,
1510                                       CollPair *collpair)
1511 {
1512   const float *co1 = mverts[bp1].co, *co2 = mverts[bp2].co, *co3 = mverts[bp3].co;
1513   float lambda /*, distance1 */, distance2;
1514   float facenor[3], v1p1[3], v1p2[3];
1515   float w[3];
1516
1517   if (!cloth_point_face_collision_params(p1, p2, co1, co2, co3, facenor, &lambda, w)) {
1518     return collpair;
1519   }
1520
1521   sub_v3_v3v3(v1p1, p1, co1);
1522   //  distance1 = dot_v3v3(v1p1, facenor);
1523   sub_v3_v3v3(v1p2, p2, co1);
1524   distance2 = dot_v3v3(v1p2, facenor);
1525   //  if (distance2 > epsilon || (distance1 < 0.0f && distance2 < 0.0f))
1526   if (distance2 > epsilon) {
1527     return collpair;
1528   }
1529
1530   collpair->face1 = index_cloth; /* XXX actually not a face, but equivalent index for point */
1531   collpair->face2 = index_coll;
1532   collpair->ap1 = index_cloth;
1533   collpair->ap2 = collpair->ap3 = -1; /* unused */
1534   collpair->bp1 = bp1;
1535   collpair->bp2 = bp2;
1536   collpair->bp3 = bp3;
1537
1538   /* note: using the second point here, which is
1539    * the current updated position that needs to be corrected
1540    */
1541   copy_v3_v3(collpair->pa, p2);
1542   collpair->distance = distance2;
1543   mul_v3_v3fl(collpair->vector, facenor, -distance2);
1544
1545   interp_v3_v3v3v3(collpair->pb, co1, co2, co3, w);
1546
1547   copy_v3_v3(collpair->normal, facenor);
1548   collpair->time = lambda;
1549   collpair->flag = 0;
1550
1551   collpair++;
1552   return collpair;
1553 }
1554
1555 /* Determines collisions on overlap,
1556  * collisions are written to collpair[i] and collision+number_collision_found is returned. */
1557 static CollPair *cloth_point_collision(ModifierData *md1,
1558                                        ModifierData *md2,
1559                                        BVHTreeOverlap *overlap,
1560                                        float epsilon,
1561                                        CollPair *collpair,
1562                                        float UNUSED(dt))
1563 {
1564   ClothModifierData *clmd = (ClothModifierData *)md1;
1565   CollisionModifierData *collmd = (CollisionModifierData *)md2;
1566   /* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
1567   ClothVertex *vert = NULL;
1568   const MVertTri *vt;
1569   const MVert *mverts = collmd->current_x;
1570
1571   vert = &clmd->clothObject->verts[overlap->indexA];
1572   vt = &collmd->tri[overlap->indexB];
1573
1574   collpair = cloth_point_collpair(vert->tx,
1575                                   vert->x,
1576                                   mverts,
1577                                   vt->tri[0],
1578                                   vt->tri[1],
1579                                   vt->tri[2],
1580                                   overlap->indexA,
1581                                   overlap->indexB,
1582                                   epsilon,
1583                                   collpair);
1584
1585   return collpair;
1586 }
1587
1588 static void cloth_points_objcollisions_nearcheck(ClothModifierData *clmd,
1589                                                  CollisionModifierData *collmd,
1590                                                  CollPair **collisions,
1591                                                  CollPair **collisions_index,
1592                                                  int numresult,
1593                                                  BVHTreeOverlap *overlap,
1594                                                  float epsilon,
1595                                                  double dt)
1596 {
1597   int i;
1598
1599   /* can return 2 collisions in total */
1600   *collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * numresult * 2, "collision array");
1601   *collisions_index = *collisions;
1602
1603   for (i = 0; i < numresult; i++) {
1604     *collisions_index = cloth_point_collision(
1605         (ModifierData *)clmd, (ModifierData *)collmd, overlap + i, epsilon, *collisions_index, dt);
1606   }
1607 }
1608
1609 void cloth_find_point_contacts(Depsgraph *depsgraph,
1610                                Object *ob,
1611                                ClothModifierData *clmd,
1612                                float step,
1613                                float dt,
1614                                ColliderContacts **r_collider_contacts,
1615                                int *r_totcolliders)
1616 {
1617   Cloth *cloth = clmd->clothObject;
1618   BVHTree *cloth_bvh;
1619   unsigned int i = 0, mvert_num = 0;
1620   ClothVertex *verts = NULL;
1621
1622   ColliderContacts *collider_contacts;
1623
1624   Object **collobjs = NULL;
1625   unsigned int numcollobj = 0;
1626
1627   verts = cloth->verts;
1628   mvert_num = cloth->mvert_num;
1629
1630   ////////////////////////////////////////////////////////////
1631   // static collisions
1632   ////////////////////////////////////////////////////////////
1633
1634   /* Check we do have collision objects to test against, before doing anything else. */
1635   collobjs = BKE_collision_objects_create(
1636       depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
1637   if (!collobjs) {
1638     *r_collider_contacts = NULL;
1639     *r_totcolliders = 0;
1640     return;
1641   }
1642
1643   // create temporary cloth points bvh
1644   cloth_bvh = BLI_bvhtree_new(mvert_num, clmd->coll_parms->epsilon, 4, 6);
1645   /* fill tree */
1646   for (i = 0; i < mvert_num; i++) {
1647     float co[6];
1648
1649     copy_v3_v3(&co[0 * 3], verts[i].x);
1650     copy_v3_v3(&co[1 * 3], verts[i].tx);
1651
1652     BLI_bvhtree_insert(cloth_bvh, i, co, 2);
1653   }
1654   /* balance tree */
1655   BLI_bvhtree_balance(cloth_bvh);
1656
1657   /* move object to position (step) in time */
1658   for (i = 0; i < numcollobj; i++) {
1659     Object *collob = collobjs[i];
1660     CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(
1661         collob, eModifierType_Collision);
1662     if (!collmd->bvhtree) {
1663       continue;
1664     }
1665
1666     /* move object to position (step) in time */
1667     collision_move_object(collmd, step + dt, step);
1668   }
1669
1670   collider_contacts = MEM_callocN(sizeof(ColliderContacts) * numcollobj, "CollPair");
1671
1672   // check all collision objects
1673   for (i = 0; i < numcollobj; i++) {
1674     ColliderContacts *ct = collider_contacts + i;
1675     Object *collob = collobjs[i];
1676     CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(
1677         collob, eModifierType_Collision);
1678     BVHTreeOverlap *overlap;
1679     unsigned int result = 0;
1680     float epsilon;
1681
1682     ct->ob = collob;
1683     ct->collmd = collmd;
1684     ct->collisions = NULL;
1685     ct->totcollisions = 0;
1686
1687     if (!collmd->bvhtree) {
1688       continue;
1689     }
1690
1691     /* search for overlapping collision pairs */
1692     overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL);
1693     epsilon = BLI_bvhtree_get_epsilon(collmd->bvhtree);
1694
1695     // go to next object if no overlap is there
1696     if (result && overlap) {
1697       CollPair *collisions_index;
1698
1699       /* check if collisions really happen (costly near check) */
1700       cloth_points_objcollisions_nearcheck(
1701           clmd, collmd, &ct->collisions, &collisions_index, result, overlap, epsilon, dt);
1702       ct->totcollisions = (int)(collisions_index - ct->collisions);
1703
1704       /* Resolve nearby collisions. */
1705 #if 0
1706       ret += cloth_points_objcollisions_resolve(
1707           clmd, collmd, collob->pd, collisions[i], collisions_index[i], dt);
1708 #endif
1709     }
1710
1711     if (overlap) {
1712       MEM_freeN(overlap);
1713     }
1714   }
1715
1716   BKE_collision_objects_free(collobjs);
1717
1718   BLI_bvhtree_free(cloth_bvh);
1719
1720   ////////////////////////////////////////////////////////////
1721   // update positions
1722   // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1723   ////////////////////////////////////////////////////////////
1724
1725   // verts come from clmd
1726   for (i = 0; i < mvert_num; i++) {
1727     if (clmd->sim_parms->vgroup_mass > 0) {
1728       if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
1729         continue;
1730       }
1731     }
1732
1733     add_v3_v3v3(verts[i].tx, verts[i].txold, verts[i].tv);
1734   }
1735   ////////////////////////////////////////////////////////////
1736
1737   *r_collider_contacts = collider_contacts;
1738   *r_totcolliders = numcollobj;
1739 }
1740
1741 void cloth_free_contacts(ColliderContacts *collider_contacts, int totcolliders)
1742 {
1743   if (collider_contacts) {
1744     int i;
1745     for (i = 0; i < totcolliders; ++i) {
1746       ColliderContacts *ct = collider_contacts + i;
1747       if (ct->collisions) {
1748         MEM_freeN(ct->collisions);
1749       }
1750     }
1751     MEM_freeN(collider_contacts);
1752   }
1753 }