style cleanup
[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 "BKE_cloth.h"
36
37 #include "DNA_cloth_types.h"
38 #include "DNA_group_types.h"
39 #include "DNA_mesh_types.h"
40 #include "DNA_object_types.h"
41 #include "DNA_object_force.h"
42 #include "DNA_scene_types.h"
43 #include "DNA_meshdata_types.h"
44
45 #include "BLI_utildefines.h"
46 #include "BLI_blenlib.h"
47 #include "BLI_math.h"
48 #include "BLI_edgehash.h"
49 #include "BLI_utildefines.h"
50 #include "BLI_ghash.h"
51 #include "BLI_memarena.h"
52 #include "BLI_rand.h"
53
54 #include "BKE_DerivedMesh.h"
55 #include "BKE_global.h"
56 #include "BKE_scene.h"
57 #include "BKE_mesh.h"
58 #include "BKE_object.h"
59 #include "BKE_modifier.h"
60
61 #include "BKE_DerivedMesh.h"
62 #ifdef USE_BULLET
63 #include "Bullet-C-Api.h"
64 #endif
65 #include "BLI_kdopbvh.h"
66 #include "BKE_collision.h"
67
68 #ifdef WITH_ELTOPO
69 #include "eltopo-capi.h"
70 #endif
71
72
73 /***********************************
74 Collision modifier code start
75 ***********************************/
76
77 /* step is limited from 0 (frame start position) to 1 (frame end position) */
78 void collision_move_object(CollisionModifierData *collmd, float step, float prevstep)
79 {
80         float tv[3] = {0, 0, 0};
81         unsigned int i = 0;
82
83         for ( i = 0; i < collmd->numverts; 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 ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
91 }
92
93 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
94 {
95         BVHTree *tree;
96         float co[12];
97         unsigned int i;
98         MFace *tface = mfaces;
99
100         tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
101
102         // fill tree
103         for ( i = 0; i < numfaces; i++, tface++ ) {
104                 copy_v3_v3 ( &co[0*3], x[tface->v1].co );
105                 copy_v3_v3 ( &co[1*3], x[tface->v2].co );
106                 copy_v3_v3 ( &co[2*3], x[tface->v3].co );
107                 if ( tface->v4 )
108                         copy_v3_v3 ( &co[3*3], x[tface->v4].co );
109
110                 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
111         }
112
113         // balance tree
114         BLI_bvhtree_balance ( tree );
115
116         return tree;
117 }
118
119 void bvhtree_update_from_mvert(BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
120 {
121         int i;
122         MFace *mfaces = faces;
123         float co[12], co_moving[12];
124         int ret = 0;
125
126         if ( !bvhtree )
127                 return;
128
129         if ( x ) {
130                 for ( i = 0; i < numfaces; i++, mfaces++ ) {
131                         copy_v3_v3 ( &co[0*3], x[mfaces->v1].co );
132                         copy_v3_v3 ( &co[1*3], x[mfaces->v2].co );
133                         copy_v3_v3 ( &co[2*3], x[mfaces->v3].co );
134                         if ( mfaces->v4 )
135                                 copy_v3_v3 ( &co[3*3], x[mfaces->v4].co );
136
137                         // copy new locations into array
138                         if ( moving && xnew ) {
139                                 // update moving positions
140                                 copy_v3_v3 ( &co_moving[0*3], xnew[mfaces->v1].co );
141                                 copy_v3_v3 ( &co_moving[1*3], xnew[mfaces->v2].co );
142                                 copy_v3_v3 ( &co_moving[2*3], xnew[mfaces->v3].co );
143                                 if ( mfaces->v4 )
144                                         copy_v3_v3 ( &co_moving[3*3], xnew[mfaces->v4].co );
145
146                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
147                         }
148                         else {
149                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
150                         }
151
152                         // check if tree is already full
153                         if ( !ret )
154                                 break;
155                 }
156
157                 BLI_bvhtree_update_tree ( bvhtree );
158         }
159 }
160
161 /***********************************
162 Collision modifier code end
163 ***********************************/
164
165 // w3 is not perfect
166 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
167 {
168         double  tempV1[3], tempV2[3], tempV4[3];
169         double  a, b, c, d, e, f;
170
171         VECSUB ( tempV1, p1, p3 );
172         VECSUB ( tempV2, p2, p3 );
173         VECSUB ( tempV4, pv, p3 );
174
175         a = INPR ( tempV1, tempV1 );
176         b = INPR ( tempV1, tempV2 );
177         c = INPR ( tempV2, tempV2 );
178         e = INPR ( tempV1, tempV4 );
179         f = INPR ( tempV2, tempV4 );
180
181         d = ( a * c - b * b );
182
183         if ( ABS ( d ) < (double)ALMOST_ZERO ) {
184                 *w1 = *w2 = *w3 = 1.0 / 3.0;
185                 return;
186         }
187
188         w1[0] = ( float ) ( ( e * c - b * f ) / d );
189
190         if ( w1[0] < 0 )
191                 w1[0] = 0;
192
193         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
194
195         if ( w2[0] < 0 )
196                 w2[0] = 0;
197
198         w3[0] = 1.0f - w1[0] - w2[0];
199 }
200
201 #ifdef __GNUC__
202 #  pragma GCC diagnostic push
203 #  pragma GCC diagnostic ignored "-Wdouble-promotion"
204 #endif
205
206 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
207 {
208         zero_v3(to);
209         VECADDMUL(to, v1, w1);
210         VECADDMUL(to, v2, w2);
211         VECADDMUL(to, v3, w3);
212 }
213
214 static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
215 {
216         int result = 0;
217         Cloth *cloth1;
218         float w1, w2, w3, u1, u2, u3;
219         float v1[3], v2[3], relativeVelocity[3];
220         float magrelVel;
221         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
222
223         cloth1 = clmd->clothObject;
224
225         for ( ; collpair != collision_end; collpair++ ) {
226                 float i1[3], i2[3], i3[3];
227
228                 zero_v3(i1);
229                 zero_v3(i2);
230                 zero_v3(i3);
231
232                 /* only handle static collisions here */
233                 if ( collpair->flag & COLLISION_IN_FUTURE )
234                         continue;
235
236                 /* compute barycentric coordinates for both collision points */
237                 collision_compute_barycentric ( collpair->pa,
238                         cloth1->verts[collpair->ap1].txold,
239                         cloth1->verts[collpair->ap2].txold,
240                         cloth1->verts[collpair->ap3].txold,
241                         &w1, &w2, &w3 );
242
243                 /* was: txold */
244                 collision_compute_barycentric ( collpair->pb,
245                         collmd->current_x[collpair->bp1].co,
246                         collmd->current_x[collpair->bp2].co,
247                         collmd->current_x[collpair->bp3].co,
248                         &u1, &u2, &u3 );
249
250                 /* Calculate relative "velocity". */
251                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
252
253                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
254
255                 sub_v3_v3v3(relativeVelocity, v2, v1);
256
257                 /* Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). */
258                 magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
259
260                 /* printf("magrelVel: %f\n", magrelVel); */
261
262                 /* Calculate masses of points.
263                  * TODO */
264
265                 /* If v_n_mag < 0 the edges are approaching each other. */
266                 if ( magrelVel > ALMOST_ZERO ) {
267                         /* Calculate Impulse magnitude to stop all motion in normal direction. */
268                         float magtangent = 0, repulse = 0, d = 0;
269                         double impulse = 0.0;
270                         float vrel_t_pre[3];
271                         float temp[3], spf;
272
273                         /* calculate tangential velocity */
274                         copy_v3_v3 ( temp, collpair->normal );
275                         mul_v3_fl(temp, magrelVel);
276                         sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
277
278                         /* Decrease in magnitude of relative tangential velocity due to coulomb friction
279                          * in original formula "magrelVel" should be the "change of relative velocity in normal direction" */
280                         magtangent = min_ff(clmd->coll_parms->friction * 0.01f * magrelVel, sqrtf(dot_v3v3(vrel_t_pre, vrel_t_pre)));
281
282                         /* Apply friction impulse. */
283                         if ( magtangent > ALMOST_ZERO ) {
284                                 normalize_v3(vrel_t_pre);
285
286                                 impulse = magtangent / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* 2.0 * */
287                                 VECADDMUL ( i1, vrel_t_pre, w1 * impulse );
288                                 VECADDMUL ( i2, vrel_t_pre, w2 * impulse );
289                                 VECADDMUL ( i3, vrel_t_pre, w3 * impulse );
290                         }
291
292                         /* Apply velocity stopping impulse
293                          * I_c = m * v_N / 2.0
294                          * no 2.0 * magrelVel normally, but looks nicer DG */
295                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
296
297                         VECADDMUL ( i1, collpair->normal, w1 * impulse );
298                         cloth1->verts[collpair->ap1].impulse_count++;
299
300                         VECADDMUL ( i2, collpair->normal, w2 * impulse );
301                         cloth1->verts[collpair->ap2].impulse_count++;
302
303                         VECADDMUL ( i3, collpair->normal, w3 * impulse );
304                         cloth1->verts[collpair->ap3].impulse_count++;
305
306                         /* Apply repulse impulse if distance too short
307                          * I_r = -min(dt*kd, m(0, 1d/dt - v_n))
308                          * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this:
309                          * v += impulse; x_new = x + v;
310                          * We don't use dt!!
311                          * DG TODO: Fix usage of dt here! */
312                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
313
314                         d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
315                         if ( ( magrelVel < 0.1f*d*spf ) && ( d > ALMOST_ZERO ) ) {
316                                 repulse = MIN2 ( d*1.0f/spf, 0.1f*d*spf - magrelVel );
317
318                                 /* stay on the safe side and clamp repulse */
319                                 if ( impulse > ALMOST_ZERO )
320                                         repulse = min_ff( repulse, 5.0*impulse );
321                                 repulse = max_ff(impulse, repulse);
322
323                                 impulse = repulse / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* original 2.0 / 0.25 */
324                                 VECADDMUL ( i1, collpair->normal,  impulse );
325                                 VECADDMUL ( i2, collpair->normal,  impulse );
326                                 VECADDMUL ( i3, collpair->normal,  impulse );
327                         }
328
329                         result = 1;
330                 }
331                 else {
332                         /* Apply repulse impulse if distance too short
333                          * I_r = -min(dt*kd, max(0, 1d/dt - v_n))
334                          * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this:
335                          * v += impulse; x_new = x + v;
336                          * We don't use dt!! */
337                         float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
338
339                         float d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - (float)collpair->distance;
340                         if ( d > ALMOST_ZERO) {
341                                 /* stay on the safe side and clamp repulse */
342                                 float repulse = d*1.0f/spf;
343
344                                 float impulse = repulse / ( 3.0f * ( 1.0f + w1*w1 + w2*w2 + w3*w3 )); /* original 2.0 / 0.25 */
345
346                                 VECADDMUL ( i1, collpair->normal,  impulse );
347                                 VECADDMUL ( i2, collpair->normal,  impulse );
348                                 VECADDMUL ( i3, collpair->normal,  impulse );
349
350                                 cloth1->verts[collpair->ap1].impulse_count++;
351                                 cloth1->verts[collpair->ap2].impulse_count++;
352                                 cloth1->verts[collpair->ap3].impulse_count++;
353
354                                 result = 1;
355                         }
356                 }
357
358                 if (result) {
359                         int i = 0;
360
361                         for (i = 0; i < 3; i++) {
362                                 if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[i]) < ABS(i1[i]))
363                                         cloth1->verts[collpair->ap1].impulse[i] = i1[i];
364
365                                 if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[i]) < ABS(i2[i]))
366                                         cloth1->verts[collpair->ap2].impulse[i] = i2[i];
367
368                                 if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[i]) < ABS(i3[i]))
369                                         cloth1->verts[collpair->ap3].impulse[i] = i3[i];
370                         }
371                 }
372         }
373         return result;
374 }
375
376 #ifdef __GNUC__
377 #  pragma GCC diagnostic pop
378 #endif
379
380 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
381 static CollPair* cloth_collision(ModifierData *md1, ModifierData *md2,
382                                  BVHTreeOverlap *overlap, CollPair *collpair, float UNUSED(dt))
383 {
384         ClothModifierData *clmd = (ClothModifierData *)md1;
385         CollisionModifierData *collmd = (CollisionModifierData *) md2;
386         /* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
387         MFace *face1=NULL, *face2 = NULL;
388 #ifdef USE_BULLET
389         ClothVertex *verts1 = clmd->clothObject->verts;
390 #endif
391         double distance = 0;
392         float epsilon1 = clmd->coll_parms->epsilon;
393         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
394         int i;
395
396         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
397         face2 = & ( collmd->mfaces[overlap->indexB] );
398
399         // check all 4 possible collisions
400         for ( i = 0; i < 4; i++ ) {
401                 if ( i == 0 ) {
402                         // fill faceA
403                         collpair->ap1 = face1->v1;
404                         collpair->ap2 = face1->v2;
405                         collpair->ap3 = face1->v3;
406
407                         // fill faceB
408                         collpair->bp1 = face2->v1;
409                         collpair->bp2 = face2->v2;
410                         collpair->bp3 = face2->v3;
411                 }
412                 else if ( i == 1 ) {
413                         if ( face1->v4 ) {
414                                 // fill faceA
415                                 collpair->ap1 = face1->v1;
416                                 collpair->ap2 = face1->v3;
417                                 collpair->ap3 = face1->v4;
418
419                                 // fill faceB
420                                 collpair->bp1 = face2->v1;
421                                 collpair->bp2 = face2->v2;
422                                 collpair->bp3 = face2->v3;
423                         }
424                         else {
425                                 i++;
426                         }
427                 }
428                 if ( i == 2 ) {
429                         if ( face2->v4 ) {
430                                 // fill faceA
431                                 collpair->ap1 = face1->v1;
432                                 collpair->ap2 = face1->v2;
433                                 collpair->ap3 = face1->v3;
434
435                                 // fill faceB
436                                 collpair->bp1 = face2->v1;
437                                 collpair->bp2 = face2->v4;
438                                 collpair->bp3 = face2->v3;
439                         }
440                         else {
441                                 break;
442                         }
443                 }
444                 else if ( i == 3 ) {
445                         if ( face1->v4 && face2->v4 ) {
446                                 // fill faceA
447                                 collpair->ap1 = face1->v1;
448                                 collpair->ap2 = face1->v3;
449                                 collpair->ap3 = face1->v4;
450
451                                 // fill faceB
452                                 collpair->bp1 = face2->v1;
453                                 collpair->bp2 = face2->v3;
454                                 collpair->bp3 = face2->v4;
455                         }
456                         else {
457                                 break;
458                         }
459                 }
460                 
461 #ifdef USE_BULLET
462                 // calc distance + normal
463                 distance = plNearestPoints (
464                         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 );
465 #else
466                 // just be sure that we don't add anything
467                 distance = 2.0 * (double)( epsilon1 + epsilon2 + ALMOST_ZERO );
468 #endif
469
470                 // distance -1 means no collision result
471                 if (distance != -1.0 && (distance <= (double)(epsilon1 + epsilon2 + ALMOST_ZERO))) {
472                         normalize_v3_v3(collpair->normal, collpair->vector);
473
474                         collpair->distance = distance;
475                         collpair->flag = 0;
476                         collpair++;
477                 }/*
478                 else {
479                         float w1, w2, w3, u1, u2, u3;
480                         float v1[3], v2[3], relativeVelocity[3];
481
482                         // calc relative velocity
483                         
484                         // compute barycentric coordinates for both collision points
485                         collision_compute_barycentric ( collpair->pa,
486                         verts1[collpair->ap1].txold,
487                         verts1[collpair->ap2].txold,
488                         verts1[collpair->ap3].txold,
489                         &w1, &w2, &w3 );
490
491                         // was: txold
492                         collision_compute_barycentric ( collpair->pb,
493                         collmd->current_x[collpair->bp1].co,
494                         collmd->current_x[collpair->bp2].co,
495                         collmd->current_x[collpair->bp3].co,
496                         &u1, &u2, &u3 );
497
498                         // Calculate relative "velocity".
499                         collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
500
501                         collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
502
503                         sub_v3_v3v3(relativeVelocity, v2, v1);
504
505                         if (sqrt(dot_v3v3(relativeVelocity, relativeVelocity)) >= distance)
506                         {
507                                 // check for collision in the future
508                                 collpair->flag |= COLLISION_IN_FUTURE;
509                                 collpair++;
510                         }
511                 }*/
512         }
513         return collpair;
514 }
515
516 static void add_collision_object(Object ***objs, unsigned int *numobj, unsigned int *maxobj, Object *ob, Object *self, int level, unsigned int modifier_type)
517 {
518         CollisionModifierData *cmd= NULL;
519
520         if (ob == self)
521                 return;
522
523         /* only get objects with collision modifier */
524         if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) || (modifier_type != eModifierType_Collision))
525                 cmd= (CollisionModifierData *)modifiers_findByType(ob, modifier_type);
526         
527         if (cmd) {
528                 /* extend array */
529                 if (*numobj >= *maxobj) {
530                         *maxobj *= 2;
531                         *objs= MEM_reallocN(*objs, sizeof(Object *)*(*maxobj));
532                 }
533                 
534                 (*objs)[*numobj] = ob;
535                 (*numobj)++;
536         }
537
538         /* objects in dupli groups, one level only for now */
539         if (ob->dup_group && level == 0) {
540                 GroupObject *go;
541                 Group *group= ob->dup_group;
542
543                 /* add objects */
544                 for (go= group->gobject.first; go; go= go->next)
545                         add_collision_object(objs, numobj, maxobj, go->ob, self, level+1, modifier_type);
546         }
547 }
548
549 // return all collision objects in scene
550 // collision object will exclude self 
551 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj, unsigned int modifier_type)
552 {
553         Base *base;
554         Object **objs;
555         GroupObject *go;
556         unsigned int numobj= 0, maxobj= 100;
557         
558         objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
559
560         /* gather all collision objects */
561         if (group) {
562                 /* use specified group */
563                 for (go= group->gobject.first; go; go= go->next)
564                         add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0, modifier_type);
565         }
566         else {
567                 Scene *sce_iter;
568                 /* add objects in same layer in scene */
569                 for (SETLOOPER(scene, sce_iter, base)) {
570                         /* Need to check for active layers, too.
571                         Otherwise this check fails if the objects are not on the same layer - DG */
572                         if ((base->lay & self->lay) || (base->lay & scene->lay))
573                                 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0, modifier_type);
574
575                 }
576         }
577
578         *numcollobj= numobj;
579
580         return objs;
581 }
582
583 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
584 {
585         CollisionModifierData *cmd= NULL;
586         ColliderCache *col;
587
588         if (ob == self)
589                 return;
590
591         if (ob->pd && ob->pd->deflect)
592                 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
593         
594         if (cmd && cmd->bvhtree) {
595                 if (*objs == NULL)
596                         *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
597
598                 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
599                 col->ob = ob;
600                 col->collmd = cmd;
601                 /* make sure collider is properly set up */
602                 collision_move_object(cmd, 1.0, 0.0);
603                 BLI_addtail(*objs, col);
604         }
605
606         /* objects in dupli groups, one level only for now */
607         if (ob->dup_group && level == 0) {
608                 GroupObject *go;
609                 Group *group= ob->dup_group;
610
611                 /* add objects */
612                 for (go= group->gobject.first; go; go= go->next)
613                         add_collider_cache_object(objs, go->ob, self, level+1);
614         }
615 }
616
617 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
618 {
619         GroupObject *go;
620         ListBase *objs= NULL;
621         
622         /* add object in same layer in scene */
623         if (group) {
624                 for (go= group->gobject.first; go; go= go->next)
625                         add_collider_cache_object(&objs, go->ob, self, 0);
626         }
627         else {
628                 Scene *sce_iter;
629                 Base *base;
630
631                 /* add objects in same layer in scene */
632                 for (SETLOOPER(scene, sce_iter, base)) {
633                         if (!self || (base->lay & self->lay))
634                                 add_collider_cache_object(&objs, base->object, self, 0);
635
636                 }
637         }
638
639         return objs;
640 }
641
642 void free_collider_cache(ListBase **colliders)
643 {
644         if (*colliders) {
645                 BLI_freelistN(*colliders);
646                 MEM_freeN(*colliders);
647                 *colliders = NULL;
648         }
649 }
650
651
652 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd,
653         CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap, double dt)
654 {
655         int i;
656         
657         *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 64, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
658         *collisions_index = *collisions;
659
660         for ( i = 0; i < numresult; i++ ) {
661                 *collisions_index = cloth_collision ( (ModifierData *)clmd, (ModifierData *)collmd,
662                                                       overlap+i, *collisions_index, dt );
663         }
664 }
665
666 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
667 {
668         Cloth *cloth = clmd->clothObject;
669         int i=0, j = 0, /*numfaces = 0, */ numverts = 0;
670         ClothVertex *verts = NULL;
671         int ret = 0;
672         int result = 0;
673         
674         numverts = clmd->clothObject->numverts;
675         verts = cloth->verts;
676         
677         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
678         result = 1;
679         for ( j = 0; j < 2; j++ ) { /* 5 is just a value that ensures convergence */
680                 result = 0;
681
682                 if ( collmd->bvhtree ) {
683                         result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
684
685                         // apply impulses in parallel
686                         if (result) {
687                                 for (i = 0; i < numverts; i++) {
688                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
689                                         if (verts[i].impulse_count) {
690                                                 // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
691                                                 VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
692                                                 zero_v3(verts[i].impulse);
693                                                 verts[i].impulse_count = 0;
694
695                                                 ret++;
696                                         }
697                                 }
698                         }
699                 }
700
701                 if (!result) {
702                         break;
703                 }
704         }
705         return ret;
706 }
707
708 // cloth - object collisions
709 int cloth_bvh_objcollision(Object *ob, ClothModifierData * clmd, float step, float dt )
710 {
711         Cloth *cloth= clmd->clothObject;
712         BVHTree *cloth_bvh= cloth->bvhtree;
713         unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ numverts = 0, k, l, j;
714         int rounds = 0; // result counts applied collisions; ic is for debug output;
715         ClothVertex *verts = NULL;
716         int ret = 0, ret2 = 0;
717         Object **collobjs = NULL;
718         unsigned int numcollobj = 0;
719
720         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
721                 return 0;
722         
723         verts = cloth->verts;
724         /* numfaces = cloth->numfaces; */ /* UNUSED */
725         numverts = cloth->numverts;
726
727         ////////////////////////////////////////////////////////////
728         // static collisions
729         ////////////////////////////////////////////////////////////
730
731         // update cloth bvh
732         bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
733         bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
734         
735         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
736         
737         if (!collobjs)
738                 return 0;
739
740         /* move object to position (step) in time */
741         for (i = 0; i < numcollobj; i++) {
742                 Object *collob= collobjs[i];
743                 CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
744
745                 if (!collmd->bvhtree)
746                         continue;
747
748                 /* move object to position (step) in time */
749                 collision_move_object ( collmd, step + dt, step );
750         }
751
752         do {
753                 CollPair **collisions, **collisions_index;
754                 
755                 ret2 = 0;
756
757                 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
758                 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
759                 
760                 // check all collision objects
761                 for (i = 0; i < numcollobj; i++) {
762                         Object *collob= collobjs[i];
763                         CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
764                         BVHTreeOverlap *overlap = NULL;
765                         unsigned int result = 0;
766                         
767                         if (!collmd->bvhtree)
768                                 continue;
769                         
770                         /* search for overlapping collision pairs */
771                         overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
772                                 
773                         // go to next object if no overlap is there
774                         if ( result && overlap ) {
775                                 /* check if collisions really happen (costly near check) */
776                                 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], 
777                                         &collisions_index[i], result, overlap, dt/(float)clmd->coll_parms->loop_count);
778                         
779                                 // resolve nearby collisions
780                                 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
781                                 ret2 += ret;
782                         }
783
784                         if ( overlap )
785                                 MEM_freeN ( overlap );
786                 }
787                 rounds++;
788                 
789                 for (i = 0; i < numcollobj; i++) {
790                         if ( collisions[i] ) MEM_freeN ( collisions[i] );
791                 }
792                         
793                 MEM_freeN(collisions);
794                 MEM_freeN(collisions_index);
795
796                 ////////////////////////////////////////////////////////////
797                 // update positions
798                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
799                 ////////////////////////////////////////////////////////////
800
801                 // verts come from clmd
802                 for ( i = 0; i < numverts; i++ ) {
803                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
804                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
805                                         continue;
806                                 }
807                         }
808
809                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
810                 }
811                 ////////////////////////////////////////////////////////////
812                 
813                 
814                 ////////////////////////////////////////////////////////////
815                 // Test on *simple* selfcollisions
816                 ////////////////////////////////////////////////////////////
817                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF ) {
818                         for (l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++) {
819                                 /* TODO: add coll quality rounds again */
820                                 BVHTreeOverlap *overlap = NULL;
821                                 unsigned int result = 0;
822         
823                                 // collisions = 1;
824                                 verts = cloth->verts; // needed for openMP
825         
826                                 /* numfaces = cloth->numfaces; */ /* UNUSED */
827                                 numverts = cloth->numverts;
828         
829                                 verts = cloth->verts;
830         
831                                 if ( cloth->bvhselftree ) {
832                                         // search for overlapping collision pairs
833                                         overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
834         
835         // #pragma omp parallel for private(k, i, j) schedule(static)
836                                         for ( k = 0; k < result; k++ ) {
837                                                 float temp[3];
838                                                 float length = 0;
839                                                 float mindistance;
840         
841                                                 i = overlap[k].indexA;
842                                                 j = overlap[k].indexB;
843         
844                                                 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
845         
846                                                 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
847                                                         if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) &&
848                                                              ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
849                                                         {
850                                                                 continue;
851                                                         }
852                                                 }
853
854                                                 if ((cloth->verts[i].flags & CLOTH_VERT_FLAG_NOSELFCOLL) ||
855                                                     (cloth->verts[j].flags & CLOTH_VERT_FLAG_NOSELFCOLL))
856                                                 {
857                                                         continue;
858                                                 }
859         
860                                                 sub_v3_v3v3(temp, verts[i].tx, verts[j].tx);
861         
862                                                 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
863         
864                                                 // check for adjacent points (i must be smaller j)
865                                                 if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) ) {
866                                                         continue;
867                                                 }
868         
869                                                 length = normalize_v3(temp );
870         
871                                                 if ( length < mindistance ) {
872                                                         float correction = mindistance - length;
873         
874                                                         if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
875                                                                 mul_v3_fl(temp, -correction);
876                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
877                                                         }
878                                                         else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) {
879                                                                 mul_v3_fl(temp, correction);
880                                                                 VECADD ( verts[i].tx, verts[i].tx, temp );
881                                                         }
882                                                         else {
883                                                                 mul_v3_fl(temp, correction * -0.5f);
884                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
885         
886                                                                 sub_v3_v3v3(verts[i].tx, verts[i].tx, temp);
887                                                         }
888                                                         ret = 1;
889                                                         ret2 += ret;
890                                                 }
891                                                 else {
892                                                         // check for approximated time collisions
893                                                 }
894                                         }
895         
896                                         if ( overlap )
897                                                 MEM_freeN ( overlap );
898         
899                                 }
900                         }
901                         ////////////////////////////////////////////////////////////
902
903                         ////////////////////////////////////////////////////////////
904                         // SELFCOLLISIONS: update velocities
905                         ////////////////////////////////////////////////////////////
906                         if ( ret2 ) {
907                                 for ( i = 0; i < cloth->numverts; i++ ) {
908                                         if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) {
909                                                 sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
910                                         }
911                                 }
912                         }
913                         ////////////////////////////////////////////////////////////
914                 }
915         }
916         while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
917         
918         if (collobjs)
919                 MEM_freeN(collobjs);
920
921         return 1|MIN2 ( ret, 1 );
922 }