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