Fix redundant declarations after recent changes in GPU debug
[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(Scene *scene, Object *self, Group *group, unsigned int *numcollobj, unsigned int modifier_type)
507 {
508         Base *base;
509         Object **objs;
510         GroupObject *go;
511         unsigned int numobj= 0, maxobj= 100;
512         
513         objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
514
515         /* gather all collision objects */
516         if (group) {
517                 /* use specified group */
518                 for (go= group->gobject.first; go; go= go->next)
519                         add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0, modifier_type);
520         }
521         else {
522                 Scene *sce_iter;
523                 /* add objects in same layer in scene */
524                 for (SETLOOPER(scene, sce_iter, base)) {
525                         /* Need to check for active layers, too.
526                         Otherwise this check fails if the objects are not on the same layer - DG */
527                         if ((base->lay & self->lay) || (base->lay & scene->lay))
528                                 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0, modifier_type);
529
530                 }
531         }
532
533         *numcollobj= numobj;
534
535         return objs;
536 }
537
538 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
539 {
540         CollisionModifierData *cmd= NULL;
541         ColliderCache *col;
542
543         if (ob == self)
544                 return;
545
546         if (ob->pd && ob->pd->deflect)
547                 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
548         
549         if (cmd && cmd->bvhtree) {
550                 if (*objs == NULL)
551                         *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
552
553                 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
554                 col->ob = ob;
555                 col->collmd = cmd;
556                 /* make sure collider is properly set up */
557                 collision_move_object(cmd, 1.0, 0.0);
558                 BLI_addtail(*objs, col);
559         }
560
561         /* objects in dupli groups, one level only for now */
562         if (ob->dup_group && level == 0) {
563                 GroupObject *go;
564                 Group *group= ob->dup_group;
565
566                 /* add objects */
567                 for (go= group->gobject.first; go; go= go->next)
568                         add_collider_cache_object(objs, go->ob, self, level+1);
569         }
570 }
571
572 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
573 {
574         GroupObject *go;
575         ListBase *objs= NULL;
576         
577         /* add object in same layer in scene */
578         if (group) {
579                 for (go= group->gobject.first; go; go= go->next)
580                         add_collider_cache_object(&objs, go->ob, self, 0);
581         }
582         else {
583                 Scene *sce_iter;
584                 Base *base;
585
586                 /* add objects in same layer in scene */
587                 for (SETLOOPER(scene, sce_iter, base)) {
588                         if (!self || (base->lay & self->lay))
589                                 add_collider_cache_object(&objs, base->object, self, 0);
590
591                 }
592         }
593
594         return objs;
595 }
596
597 void free_collider_cache(ListBase **colliders)
598 {
599         if (*colliders) {
600                 BLI_freelistN(*colliders);
601                 MEM_freeN(*colliders);
602                 *colliders = NULL;
603         }
604 }
605
606
607 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd,
608         CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap, double dt)
609 {
610         int i;
611         
612         *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 4, "collision array" ); // * 4 since cloth_collision_static can return more than 1 collision
613         *collisions_index = *collisions;
614
615         for ( i = 0; i < numresult; i++ ) {
616                 *collisions_index = cloth_collision((ModifierData *)clmd, (ModifierData *)collmd,
617                                                     overlap+i, *collisions_index, dt);
618         }
619 }
620
621 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
622 {
623         Cloth *cloth = clmd->clothObject;
624         int i=0, j = 0, /*numfaces = 0, */ mvert_num = 0;
625         ClothVertex *verts = NULL;
626         int ret = 0;
627         int result = 0;
628         
629         mvert_num = clmd->clothObject->mvert_num;
630         verts = cloth->verts;
631         
632         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
633         result = 1;
634         for ( j = 0; j < 2; j++ ) { /* 5 is just a value that ensures convergence */
635                 result = 0;
636
637                 if ( collmd->bvhtree ) {
638                         result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
639
640                         // apply impulses in parallel
641                         if (result) {
642                                 for (i = 0; i < mvert_num; i++) {
643                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
644                                         if (verts[i].impulse_count) {
645                                                 // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
646                                                 VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
647                                                 zero_v3(verts[i].impulse);
648                                                 verts[i].impulse_count = 0;
649
650                                                 ret++;
651                                         }
652                                 }
653                         }
654                 }
655
656                 if (!result) {
657                         break;
658                 }
659         }
660         return ret;
661 }
662
663 // cloth - object collisions
664 int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, float dt )
665 {
666         Cloth *cloth= clmd->clothObject;
667         BVHTree *cloth_bvh= cloth->bvhtree;
668         unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ mvert_num = 0, k, l, j;
669         int rounds = 0; // result counts applied collisions; ic is for debug output;
670         ClothVertex *verts = NULL;
671         int ret = 0, ret2 = 0;
672         Object **collobjs = NULL;
673         unsigned int numcollobj = 0;
674
675         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
676                 return 0;
677         
678         verts = cloth->verts;
679         /* numfaces = cloth->numfaces; */ /* UNUSED */
680         mvert_num = cloth->mvert_num;
681
682         ////////////////////////////////////////////////////////////
683         // static collisions
684         ////////////////////////////////////////////////////////////
685
686         // update cloth bvh
687         bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
688         bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
689         
690         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
691         
692         if (!collobjs)
693                 return 0;
694
695         /* move object to position (step) in time */
696         for (i = 0; i < numcollobj; i++) {
697                 Object *collob= collobjs[i];
698                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
699
700                 if (!collmd->bvhtree)
701                         continue;
702
703                 /* move object to position (step) in time */
704                 collision_move_object ( collmd, step + dt, step );
705         }
706
707         do {
708                 CollPair **collisions, **collisions_index;
709                 
710                 ret2 = 0;
711
712                 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
713                 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
714                 
715                 // check all collision objects
716                 for (i = 0; i < numcollobj; i++) {
717                         Object *collob= collobjs[i];
718                         CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
719                         BVHTreeOverlap *overlap = NULL;
720                         unsigned int result = 0;
721                         
722                         if (!collmd->bvhtree)
723                                 continue;
724                         
725                         /* search for overlapping collision pairs */
726                         overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL);
727                                 
728                         // go to next object if no overlap is there
729                         if ( result && overlap ) {
730                                 /* check if collisions really happen (costly near check) */
731                                 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], 
732                                         &collisions_index[i], result, overlap, dt/(float)clmd->coll_parms->loop_count);
733                         
734                                 // resolve nearby collisions
735                                 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
736                                 ret2 += ret;
737                         }
738
739                         if ( overlap )
740                                 MEM_freeN ( overlap );
741                 }
742                 rounds++;
743                 
744                 for (i = 0; i < numcollobj; i++) {
745                         if ( collisions[i] ) MEM_freeN ( collisions[i] );
746                 }
747                         
748                 MEM_freeN(collisions);
749                 MEM_freeN(collisions_index);
750
751                 ////////////////////////////////////////////////////////////
752                 // update positions
753                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
754                 ////////////////////////////////////////////////////////////
755
756                 /* verts come from clmd */
757                 for (i = 0; i < mvert_num; i++) {
758                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
759                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
760                                         continue;
761                                 }
762                         }
763
764                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
765                 }
766                 ////////////////////////////////////////////////////////////
767                 
768                 
769                 ////////////////////////////////////////////////////////////
770                 // Test on *simple* selfcollisions
771                 ////////////////////////////////////////////////////////////
772                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF ) {
773                         for (l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++) {
774                                 /* TODO: add coll quality rounds again */
775                                 BVHTreeOverlap *overlap = NULL;
776                                 unsigned int result = 0;
777         
778                                 // collisions = 1;
779                                 verts = cloth->verts; // needed for openMP
780         
781                                 /* numfaces = cloth->numfaces; */ /* UNUSED */
782                                 mvert_num = cloth->mvert_num;
783         
784                                 verts = cloth->verts;
785         
786                                 if ( cloth->bvhselftree ) {
787                                         // search for overlapping collision pairs
788                                         overlap = BLI_bvhtree_overlap(cloth->bvhselftree, cloth->bvhselftree, &result, NULL, NULL);
789         
790         // #pragma omp parallel for private(k, i, j) schedule(static)
791                                         for ( k = 0; k < result; k++ ) {
792                                                 float temp[3];
793                                                 float length = 0;
794                                                 float mindistance;
795         
796                                                 i = overlap[k].indexA;
797                                                 j = overlap[k].indexB;
798         
799                                                 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
800         
801                                                 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
802                                                         if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) &&
803                                                              ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
804                                                         {
805                                                                 continue;
806                                                         }
807                                                 }
808
809                                                 if ((cloth->verts[i].flags & CLOTH_VERT_FLAG_NOSELFCOLL) ||
810                                                     (cloth->verts[j].flags & CLOTH_VERT_FLAG_NOSELFCOLL))
811                                                 {
812                                                         continue;
813                                                 }
814         
815                                                 sub_v3_v3v3(temp, verts[i].tx, verts[j].tx);
816         
817                                                 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
818         
819                                                 if (BLI_edgeset_haskey(cloth->edgeset, i, j)) {
820                                                         continue;
821                                                 }
822         
823                                                 length = normalize_v3(temp );
824         
825                                                 if ( length < mindistance ) {
826                                                         float correction = mindistance - length;
827         
828                                                         if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
829                                                                 mul_v3_fl(temp, -correction);
830                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
831                                                         }
832                                                         else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) {
833                                                                 mul_v3_fl(temp, correction);
834                                                                 VECADD ( verts[i].tx, verts[i].tx, temp );
835                                                         }
836                                                         else {
837                                                                 mul_v3_fl(temp, correction * -0.5f);
838                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
839         
840                                                                 sub_v3_v3v3(verts[i].tx, verts[i].tx, temp);
841                                                         }
842                                                         ret = 1;
843                                                         ret2 += ret;
844                                                 }
845                                                 else {
846                                                         // check for approximated time collisions
847                                                 }
848                                         }
849         
850                                         if ( overlap )
851                                                 MEM_freeN ( overlap );
852         
853                                 }
854                         }
855                         ////////////////////////////////////////////////////////////
856
857                         ////////////////////////////////////////////////////////////
858                         // SELFCOLLISIONS: update velocities
859                         ////////////////////////////////////////////////////////////
860                         if (ret2) {
861                                 for (i = 0; i < cloth->mvert_num; i++) {
862                                         if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) {
863                                                 sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
864                                         }
865                                 }
866                         }
867                         ////////////////////////////////////////////////////////////
868                 }
869         }
870         while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
871         
872         if (collobjs)
873                 MEM_freeN(collobjs);
874
875         return 1|MIN2 ( ret, 1 );
876 }
877
878 BLI_INLINE void max_v3_v3v3(float r[3], const float a[3], const float b[3])
879 {
880         r[0] = max_ff(a[0], b[0]);
881         r[1] = max_ff(a[1], b[1]);
882         r[2] = max_ff(a[2], b[2]);
883 }
884
885 void collision_get_collider_velocity(float vel_old[3], float vel_new[3], CollisionModifierData *collmd, CollPair *collpair)
886 {
887         float u1, u2, u3;
888         
889         /* compute barycentric coordinates */
890         collision_compute_barycentric(collpair->pb,
891                                       collmd->current_x[collpair->bp1].co,
892                                       collmd->current_x[collpair->bp2].co,
893                                       collmd->current_x[collpair->bp3].co,
894                                       &u1, &u2, &u3);
895         
896         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);
897         /* XXX assume constant velocity of the collider for now */
898         copy_v3_v3(vel_old, vel_new);
899 }
900
901 static bool cloth_points_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, PartDeflect *pd,
902                                                   CollPair *collpair, CollPair *collision_end, float dt)
903 {
904         bool result = false;
905         float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - pd->pdef_sbdamp);
906         float inv_dt = 1.0f / dt;
907         Cloth *cloth1 = clmd->clothObject;
908         
909         // float w1, w2;
910         float u1, u2, u3;
911         float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3];
912         float epsilon2 = BLI_bvhtree_get_epsilon ( collmd->bvhtree );
913
914         for ( ; collpair != collision_end; collpair++ ) {
915                 float margin_distance = (float)(collpair->distance - (double)epsilon2);
916                 float impulse[3];
917                 float mag_v_rel;
918
919                 if (margin_distance > 0.0f)
920                         continue;
921
922                 zero_v3(impulse);
923
924                 /* only handle static collisions here */
925                 if ( collpair->flag & COLLISION_IN_FUTURE )
926                         continue;
927
928                 /* compute barycentric coordinates for both collision points */
929                 // w1 = 1.0f - collpair->time;
930                 // w2 = collpair->time;
931
932                 /* was: txold */
933                 collision_compute_barycentric ( collpair->pb,
934                         collmd->current_x[collpair->bp1].co,
935                         collmd->current_x[collpair->bp2].co,
936                         collmd->current_x[collpair->bp3].co,
937                         &u1, &u2, &u3 );
938
939                 /* Calculate relative velocity */
940                 copy_v3_v3(v1, cloth1->verts[collpair->ap1].tv);
941
942                 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 );
943                 /* XXX assume constant velocity of the collider for now */
944                 copy_v3_v3(v2_old, v2_new);
945
946                 sub_v3_v3v3(v_rel_old, v1, v2_old);
947                 sub_v3_v3v3(v_rel_new, v1, v2_new);
948
949                 /* normal component of the relative velocity */
950                 mag_v_rel = dot_v3v3(v_rel_old, collpair->normal);
951
952                 /**** DEBUG ****/
953                 BKE_sim_debug_data_add_dot(collpair->pa, 0.9, 0.2, 0.2, "collision", 833, collpair->face1, collpair->face2);
954                 BKE_sim_debug_data_add_dot(collpair->pb, 0.2, 0.9, 0.2, "collision", 834, collpair->face1, collpair->face2);
955                 BKE_sim_debug_data_add_line(collpair->pa, collpair->pb, 0.8, 0.8, 0.8, "collision", 835, collpair->face1, collpair->face2);
956                 /********/
957
958                 if (mag_v_rel < -ALMOST_ZERO) {
959                         float v_nor_old, v_nor_new;
960                         float v_tan_old[3], v_tan_new[3];
961                         float bounce, repulse;
962                         
963                         /* Collision response based on
964                          * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005)
965                          * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf
966                          */
967                         
968                         v_nor_old = mag_v_rel;
969                         v_nor_new = dot_v3v3(v_rel_new, collpair->normal);
970                         
971                         madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old);
972                         madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new);
973                         
974                         repulse = -margin_distance * inv_dt + dot_v3v3(v1, collpair->normal);
975                         
976                         if (margin_distance < -epsilon2) {
977                                 bounce = -v_nor_new + v_nor_old * restitution;
978                                 mul_v3_v3fl(impulse, collpair->normal, max_ff(repulse, bounce));
979                         }
980                         else {
981                                 bounce = 0.0f;
982                                 mul_v3_v3fl(impulse, collpair->normal, repulse);
983                         }
984                         cloth1->verts[collpair->ap1].impulse_count++;
985                         
986                         result = true;
987                 }
988                 
989                 if (result) {
990                         int i = 0;
991
992                         for (i = 0; i < 3; i++) {
993                                 if (cloth1->verts[collpair->ap1].impulse_count > 0 && fabsf(cloth1->verts[collpair->ap1].impulse[i]) < fabsf(impulse[i]))
994                                         cloth1->verts[collpair->ap1].impulse[i] = impulse[i];
995                         }
996                 }
997         }
998         return result;
999 }
1000
1001 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],
1002                                                   float r_nor[3], float *r_lambda, float r_w[4])
1003 {
1004         float edge1[3], edge2[3], p2face[3], p1p2[3], v0p2[3];
1005         float nor_v0p2, nor_p1p2;
1006         
1007         sub_v3_v3v3(edge1, v1, v0);
1008         sub_v3_v3v3(edge2, v2, v0);
1009         cross_v3_v3v3(r_nor, edge1, edge2);
1010         normalize_v3(r_nor);
1011         
1012         nor_v0p2 = dot_v3v3(v0p2, r_nor);
1013         madd_v3_v3v3fl(p2face, p2, r_nor, -nor_v0p2);
1014         interp_weights_face_v3(r_w, v0, v1, v2, NULL, p2face);
1015         
1016         sub_v3_v3v3(p1p2, p2, p1);
1017         sub_v3_v3v3(v0p2, p2, v0);
1018         nor_p1p2 = dot_v3v3(p1p2, r_nor);
1019         *r_lambda = (nor_p1p2 != 0.0f ? nor_v0p2 / nor_p1p2 : 0.0f);
1020         
1021         return r_w[1] >= 0.0f && r_w[2] >= 0.0f && r_w[1] + r_w[2] <= 1.0f;
1022
1023 #if 0 /* XXX this method uses the intersection point, but is broken and doesn't work well in general */
1024         float p[3], vec1[3], line[3], edge1[3], edge2[3], q[3];
1025         float a, f, u, v;
1026
1027         sub_v3_v3v3(edge1, v1, v0);
1028         sub_v3_v3v3(edge2, v2, v0);
1029         sub_v3_v3v3(line, p2, p1);
1030
1031         cross_v3_v3v3(p, line, edge2);
1032         a = dot_v3v3(edge1, p);
1033         if (a == 0.0f) return 0;
1034         f = 1.0f / a;
1035
1036         sub_v3_v3v3(vec1, p1, v0);
1037
1038         u = f * dot_v3v3(vec1, p);
1039         if ((u < 0.0f) || (u > 1.0f))
1040                 return false;
1041
1042         cross_v3_v3v3(q, vec1, edge1);
1043
1044         v = f * dot_v3v3(line, q);
1045         if ((v < 0.0f) || ((u + v) > 1.0f))
1046                 return false;
1047
1048         *r_lambda = f * dot_v3v3(edge2, q);
1049         /* don't care about 0..1 lambda range here */
1050         /*if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f))
1051          *      return 0;
1052          */
1053
1054         r_w[0] = 1.0f - u - v;
1055         r_w[1] = u;
1056         r_w[2] = v;
1057         r_w[3] = 0.0f;
1058
1059         cross_v3_v3v3(r_nor, edge1, edge2);
1060         normalize_v3(r_nor);
1061
1062         return true;
1063 #endif
1064 }
1065
1066 static CollPair *cloth_point_collpair(
1067         float p1[3], float p2[3], const MVert *mverts, int bp1, int bp2, int bp3,
1068         int index_cloth, int index_coll, float epsilon, CollPair *collpair)
1069 {
1070         const float *co1 = mverts[bp1].co, *co2 = mverts[bp2].co, *co3 = mverts[bp3].co;
1071         float lambda /*, distance1 */, distance2;
1072         float facenor[3], v1p1[3], v1p2[3];
1073         float w[4];
1074
1075         if (!cloth_point_face_collision_params(p1, p2, co1, co2, co3, facenor, &lambda, w))
1076                 return collpair;
1077         
1078         sub_v3_v3v3(v1p1, p1, co1);
1079 //      distance1 = dot_v3v3(v1p1, facenor);
1080         sub_v3_v3v3(v1p2, p2, co1);
1081         distance2 = dot_v3v3(v1p2, facenor);
1082 //      if (distance2 > epsilon || (distance1 < 0.0f && distance2 < 0.0f))
1083         if (distance2 > epsilon)
1084                 return collpair;
1085         
1086         collpair->face1 = index_cloth; /* XXX actually not a face, but equivalent index for point */
1087         collpair->face2 = index_coll;
1088         collpair->ap1 = index_cloth;
1089         collpair->ap2 = collpair->ap3 = -1; /* unused */
1090         collpair->bp1 = bp1;
1091         collpair->bp2 = bp2;
1092         collpair->bp3 = bp3;
1093         
1094         /* note: using the second point here, which is
1095          * the current updated position that needs to be corrected
1096          */
1097         copy_v3_v3(collpair->pa, p2);
1098         collpair->distance = distance2;
1099         mul_v3_v3fl(collpair->vector, facenor, -distance2);
1100         
1101         interp_v3_v3v3v3(collpair->pb, co1, co2, co3, w);
1102         
1103         copy_v3_v3(collpair->normal, facenor);
1104         collpair->time = lambda;
1105         collpair->flag = 0;
1106         
1107         collpair++;
1108         return collpair;
1109 }
1110
1111 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
1112 static CollPair *cloth_point_collision(
1113         ModifierData *md1, ModifierData *md2,
1114         BVHTreeOverlap *overlap, float epsilon, CollPair *collpair, float UNUSED(dt))
1115 {
1116         ClothModifierData *clmd = (ClothModifierData *)md1;
1117         CollisionModifierData *collmd = (CollisionModifierData *) md2;
1118         /* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
1119         ClothVertex *vert = NULL;
1120         const MVertTri *vt;
1121         const MVert *mverts = collmd->current_x;
1122
1123         vert = &clmd->clothObject->verts[overlap->indexA];
1124         vt = &collmd->tri[overlap->indexB];
1125
1126         collpair = cloth_point_collpair(
1127                 vert->tx, vert->x, mverts,
1128                 vt->tri[0], vt->tri[1], vt->tri[2],
1129                 overlap->indexA, overlap->indexB,
1130                 epsilon, collpair);
1131
1132         return collpair;
1133 }
1134
1135 static void cloth_points_objcollisions_nearcheck(ClothModifierData * clmd, CollisionModifierData *collmd,
1136                                                      CollPair **collisions, CollPair **collisions_index,
1137                                                      int numresult, BVHTreeOverlap *overlap, float epsilon, double dt)
1138 {
1139         int i;
1140         
1141         /* can return 2 collisions in total */
1142         *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 2, "collision array" );
1143         *collisions_index = *collisions;
1144
1145         for ( i = 0; i < numresult; i++ ) {
1146                 *collisions_index = cloth_point_collision((ModifierData *)clmd, (ModifierData *)collmd,
1147                                                           overlap+i, epsilon, *collisions_index, dt);
1148         }
1149 }
1150
1151 static int cloth_points_objcollisions_resolve(ClothModifierData * clmd, CollisionModifierData *collmd, PartDeflect *pd,
1152                                               CollPair *collisions, CollPair *collisions_index, float dt)
1153 {
1154         Cloth *cloth = clmd->clothObject;
1155         int i = 0, mvert_num = clmd->clothObject->mvert_num;
1156         ClothVertex *verts = cloth->verts;
1157         int ret = 0;
1158         
1159         // process all collisions
1160         if ( collmd->bvhtree ) {
1161                 bool result = cloth_points_collision_response_static(clmd, collmd, pd, collisions, collisions_index, dt);
1162                 
1163                 // apply impulses in parallel
1164                 if (result) {
1165                         for (i = 0; i < mvert_num; i++) {
1166                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
1167                                 if (verts[i].impulse_count) {
1168                                         // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1169                                         VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
1170                                         zero_v3(verts[i].impulse);
1171                                         verts[i].impulse_count = 0;
1172                                         
1173                                         ret++;
1174                                 }
1175                         }
1176                 }
1177         }
1178         
1179         return ret;
1180 }
1181
1182 // cloth - object collisions
1183 int cloth_points_objcollision(Object *ob, ClothModifierData *clmd, float step, float dt)
1184 {
1185         Cloth *cloth= clmd->clothObject;
1186         BVHTree *cloth_bvh;
1187         int rounds = 0; // result counts applied collisions; ic is for debug output;
1188         float round_dt = dt / (float)clmd->coll_parms->loop_count;
1189         unsigned int i = 0, mvert_num = 0;
1190         ClothVertex *verts = NULL;
1191         int ret = 0, ret2 = 0;
1192         Object **collobjs = NULL;
1193         unsigned int numcollobj = 0;
1194         
1195         verts = cloth->verts;
1196         mvert_num = cloth->mvert_num;
1197         
1198         ////////////////////////////////////////////////////////////
1199         // static collisions
1200         ////////////////////////////////////////////////////////////
1201         
1202         // create temporary cloth points bvh
1203         cloth_bvh = BLI_bvhtree_new(mvert_num, max_ff(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6);
1204         /* fill tree */
1205         for (i = 0; i < mvert_num; i++) {
1206                 float co[2][3];
1207                 
1208                 copy_v3_v3(co[0], verts[i].x);
1209                 copy_v3_v3(co[1], verts[i].tx);
1210                 
1211                 BLI_bvhtree_insert(cloth_bvh, i, co[0], 2);
1212         }
1213         /* balance tree */
1214         BLI_bvhtree_balance(cloth_bvh);
1215         
1216         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
1217         if (!collobjs)
1218                 return 0;
1219         
1220         /* move object to position (step) in time */
1221         for (i = 0; i < numcollobj; i++) {
1222                 Object *collob= collobjs[i];
1223                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1224                 if (!collmd->bvhtree)
1225                         continue;
1226
1227                 /* move object to position (step) in time */
1228                 collision_move_object ( collmd, step + dt, step );
1229         }
1230
1231         do {
1232                 CollPair **collisions, **collisions_index;
1233                 
1234                 ret2 = 0;
1235                 
1236                 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
1237                 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
1238                 
1239                 // check all collision objects
1240                 for (i = 0; i < numcollobj; i++) {
1241                         Object *collob= collobjs[i];
1242                         CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1243                         BVHTreeOverlap *overlap = NULL;
1244                         unsigned int result = 0;
1245                         float epsilon;
1246                         
1247                         if (!collmd->bvhtree)
1248                                 continue;
1249                         
1250                         /* search for overlapping collision pairs */
1251                         overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL);
1252                         epsilon = BLI_bvhtree_get_epsilon(collmd->bvhtree);
1253                         
1254                         // go to next object if no overlap is there
1255                         if (result && overlap) {
1256                                 /* check if collisions really happen (costly near check) */
1257                                 cloth_points_objcollisions_nearcheck(clmd, collmd, &collisions[i], &collisions_index[i],
1258                                                                      result, overlap, epsilon, round_dt);
1259                                 
1260                                 // resolve nearby collisions
1261                                 ret += cloth_points_objcollisions_resolve(clmd, collmd, collob->pd, collisions[i], collisions_index[i], round_dt);
1262                                 ret2 += ret;
1263                         }
1264                         
1265                         if (overlap)
1266                                 MEM_freeN ( overlap );
1267                 }
1268                 rounds++;
1269                 
1270                 for (i = 0; i < numcollobj; i++) {
1271                         if (collisions[i])
1272                                 MEM_freeN(collisions[i]);
1273                 }
1274                         
1275                 MEM_freeN(collisions);
1276                 MEM_freeN(collisions_index);
1277
1278                 ////////////////////////////////////////////////////////////
1279                 // update positions
1280                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1281                 ////////////////////////////////////////////////////////////
1282
1283                 // verts come from clmd
1284                 for (i = 0; i < mvert_num; i++) {
1285                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
1286                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
1287                                         continue;
1288                                 }
1289                         }
1290
1291                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
1292                 }
1293                 ////////////////////////////////////////////////////////////
1294         }
1295         while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
1296         
1297         if (collobjs)
1298                 MEM_freeN(collobjs);
1299
1300         BLI_bvhtree_free(cloth_bvh);
1301
1302         return 1|MIN2 ( ret, 1 );
1303 }
1304
1305 void cloth_find_point_contacts(Object *ob, ClothModifierData *clmd, float step, float dt,
1306                                ColliderContacts **r_collider_contacts, int *r_totcolliders)
1307 {
1308         Cloth *cloth= clmd->clothObject;
1309         BVHTree *cloth_bvh;
1310         unsigned int i = 0, mvert_num = 0;
1311         ClothVertex *verts = NULL;
1312         
1313         ColliderContacts *collider_contacts;
1314         
1315         Object **collobjs = NULL;
1316         unsigned int numcollobj = 0;
1317         
1318         verts = cloth->verts;
1319         mvert_num = cloth->mvert_num;
1320         
1321         ////////////////////////////////////////////////////////////
1322         // static collisions
1323         ////////////////////////////////////////////////////////////
1324         
1325         // create temporary cloth points bvh
1326         cloth_bvh = BLI_bvhtree_new(mvert_num, max_ff(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6);
1327         /* fill tree */
1328         for (i = 0; i < mvert_num; i++) {
1329                 float co[6];
1330                 
1331                 copy_v3_v3(&co[0*3], verts[i].x);
1332                 copy_v3_v3(&co[1*3], verts[i].tx);
1333                 
1334                 BLI_bvhtree_insert(cloth_bvh, i, co, 2);
1335         }
1336         /* balance tree */
1337         BLI_bvhtree_balance(cloth_bvh);
1338         
1339         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
1340         if (!collobjs) {
1341                 *r_collider_contacts = NULL;
1342                 *r_totcolliders = 0;
1343                 return;
1344         }
1345         
1346         /* move object to position (step) in time */
1347         for (i = 0; i < numcollobj; i++) {
1348                 Object *collob= collobjs[i];
1349                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1350                 if (!collmd->bvhtree)
1351                         continue;
1352                 
1353                 /* move object to position (step) in time */
1354                 collision_move_object ( collmd, step + dt, step );
1355         }
1356         
1357         collider_contacts = MEM_callocN(sizeof(ColliderContacts) * numcollobj, "CollPair");
1358         
1359         // check all collision objects
1360         for (i = 0; i < numcollobj; i++) {
1361                 ColliderContacts *ct = collider_contacts + i;
1362                 Object *collob= collobjs[i];
1363                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
1364                 BVHTreeOverlap *overlap;
1365                 unsigned int result = 0;
1366                 float epsilon;
1367                 
1368                 ct->ob = collob;
1369                 ct->collmd = collmd;
1370                 ct->collisions = NULL;
1371                 ct->totcollisions = 0;
1372                 
1373                 if (!collmd->bvhtree)
1374                         continue;
1375                 
1376                 /* search for overlapping collision pairs */
1377                 overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL);
1378                 epsilon = BLI_bvhtree_get_epsilon(collmd->bvhtree);
1379                 
1380                 // go to next object if no overlap is there
1381                 if (result && overlap) {
1382                         CollPair *collisions_index;
1383                         
1384                         /* check if collisions really happen (costly near check) */
1385                         cloth_points_objcollisions_nearcheck(clmd, collmd, &ct->collisions, &collisions_index,
1386                                                              result, overlap, epsilon, dt);
1387                         ct->totcollisions = (int)(collisions_index - ct->collisions);
1388                         
1389                         // resolve nearby collisions
1390 //                      ret += cloth_points_objcollisions_resolve(clmd, collmd, collob->pd, collisions[i], collisions_index[i], dt);
1391                 }
1392                 
1393                 if (overlap)
1394                         MEM_freeN(overlap);
1395         }
1396         
1397         if (collobjs)
1398                 MEM_freeN(collobjs);
1399
1400         BLI_bvhtree_free(cloth_bvh);
1401         
1402         ////////////////////////////////////////////////////////////
1403         // update positions
1404         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1405         ////////////////////////////////////////////////////////////
1406         
1407         // verts come from clmd
1408         for (i = 0; i < mvert_num; i++) {
1409                 if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) {
1410                         if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
1411                                 continue;
1412                         }
1413                 }
1414                 
1415                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1416         }
1417         ////////////////////////////////////////////////////////////
1418         
1419         *r_collider_contacts = collider_contacts;
1420         *r_totcolliders = numcollobj;
1421 }
1422
1423 void cloth_free_contacts(ColliderContacts *collider_contacts, int totcolliders)
1424 {
1425         if (collider_contacts) {
1426                 int i;
1427                 for (i = 0; i < totcolliders; ++i) {
1428                         ColliderContacts *ct = collider_contacts + i;
1429                         if (ct->collisions) {
1430                                 MEM_freeN(ct->collisions);
1431                         }
1432                 }
1433                 MEM_freeN(collider_contacts);
1434         }
1435 }