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