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