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