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