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