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