Bullet is back, but now generalized enough to allow own distance/calculation. Some...
[blender.git] / source / blender / blenkernel / intern / collision.c
1 /*  collision.c      
2
3 *
4 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version. The Blender
10 * Foundation also sells licenses for use in proprietary software under
11 * the Blender License.  See http://www.blender.org/BL/ for information
12 * about this.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software Foundation,
21 * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22 *
23 * The Original Code is Copyright (C) Blender Foundation
24 * All rights reserved.
25 *
26 * The Original Code is: all of this file.
27 *
28 * Contributor(s): none yet.
29 *
30 * ***** END GPL/BL DUAL LICENSE BLOCK *****
31 */
32
33 #include <math.h>
34 #include <stdlib.h>
35 #include <string.h>
36 #include "MEM_guardedalloc.h"
37 /* types */
38 #include "DNA_curve_types.h"
39 #include "DNA_object_types.h"
40 #include "DNA_object_force.h"
41 #include "DNA_cloth_types.h"    
42 #include "DNA_key_types.h"
43 #include "DNA_mesh_types.h"
44 #include "DNA_meshdata_types.h"
45 #include "DNA_lattice_types.h"
46 #include "DNA_scene_types.h"
47 #include "DNA_modifier_types.h"
48 #include "BLI_blenlib.h"
49 #include "BLI_arithb.h"
50 #include "BLI_edgehash.h"
51 #include "BLI_linklist.h"
52 #include "BKE_curve.h"
53 #include "BKE_deform.h"
54 #include "BKE_DerivedMesh.h"
55 #include "BKE_cdderivedmesh.h"
56 #include "BKE_displist.h"
57 #include "BKE_effect.h"
58 #include "BKE_global.h"
59 #include "BKE_mesh.h"
60 #include "BKE_object.h"
61 #include "BKE_cloth.h"
62 #include "BKE_modifier.h"
63 #include "BKE_utildefines.h"
64 #include "BKE_DerivedMesh.h"
65 #include "DNA_screen_types.h"
66 #include "BSE_headerbuttons.h"
67 #include "BIF_screen.h"
68 #include "BIF_space.h"
69 #include "mydevice.h"
70
71 #include "Bullet-C-Api.h"
72
73
74 #define DERANDOMIZE 1
75
76
77 enum TRIANGLE_MARK 
78
79         TM_MV = 1,
80  TM_ME = 2,
81  TM_V1 = 4,
82  TM_V2 = 8,
83  TM_V3 = 16,
84  TM_E1 = 32,
85  TM_E2 = 64,
86  TM_E3 = 128 
87 };
88
89 DO_INLINE int hasTriangleMark(unsigned char mark, unsigned char bit) { return mark & bit; }
90 DO_INLINE void setTriangleMark(unsigned char *mark, unsigned char bit) { mark[0] |= bit; }
91 DO_INLINE void clearTriangleMark(unsigned char *mark, unsigned char bit) { mark[0] &= ~bit; }
92
93
94 void generateTriangleMarks() 
95 {
96         /*
97         unsigned int firstEdge = 0;
98         
99         // 1. Initialization
100         memset(m_triangleMarks, 0, sizeof(unsigned char) * m_triangleCount);
101
102         // 2. The Marking Process
103         
104         // 2.1 Randomly mark triangles for covering vertices.
105         for (unsigned int v = 0; v < m_vertexCount; ++v) 
106         {
107         if (vertexCover(v) == 0) 
108         {
109
110                         // Randomly select an edge whose first triangle we're going to flag. 
111
112 #ifndef DERANDOMIZE
113         firstEdge = (unsigned int)((float)(random() & 0x7FFFFFFF) /
114         (float)(0x80000000) *
115         (float)(m_vertices[v].getEdgeCount()));
116 #endif
117         for (unsigned int ofs = 0; ofs < m_vertices[v].getEdgeCount(); ++ofs) 
118         {
119         unsigned int edgeIdx = (firstEdge + ofs) % m_vertices[v].getEdgeCount();
120         if (m_edges[m_vertices[v].getEdge(edgeIdx)].getTriangleCount())
121         setTriangleMark(m_triangleMarks[m_edges[m_vertices[v].getEdge(edgeIdx)].getTriangle(0)], TM_MV);
122 }
123 }
124 }
125         */
126         /* If the Cloth is malformed (vertices without adjacent triangles) there might still be uncovered vertices. (Bad luck.) */
127         /*
128         // 2.2 Randomly mark triangles for covering edges.
129         for (unsigned int e = 0; e < m_edgeCount; ++e) 
130         {
131         if (m_edges[e].getTriangleCount() && (edgeCover(e) == 0)) 
132         {
133 #ifndef DERANDOMIZE
134         setTriangleMark(m_triangleMarks[m_edges[e].getTriangle(static_cast<UINT32>((float)(random() & 0x7FFFFFFF) /
135         (float)(0x80000000) *
136         (float)(m_edges[e].getTriangleCount())))], TM_ME);
137 #else
138         setTriangleMark(m_triangleMarks[m_edges[e].getTriangle(0)], TM_ME);
139 #endif
140 }
141 }
142
143         
144         // 3. The Unmarking Process
145         for (unsigned int t = 0; (t < m_triangleCount); ++t) 
146         {
147         bool overCoveredVertices = true;
148         bool overCoveredEdges = true;
149         for (unsigned char i = 0; (i < 3) && (overCoveredVertices || overCoveredEdges); ++i) 
150         {
151
152         if (vertexCover(m_triangles[t].getVertex(i)) == 1)
153         overCoveredVertices = false;
154         if (edgeCover(m_triangles[t].getEdge(i)) == 1)
155         overCoveredEdges = false;
156
157         assert(vertexCover(m_triangles[t].getVertex(i)) > 0);
158         assert(edgeCover(m_triangles[t].getEdge(i)) > 0);
159 }
160         if (overCoveredVertices)
161         clearTriangleMark(m_triangleMarks[t], TM_MV);
162         if (overCoveredEdges)
163         clearTriangleMark(m_triangleMarks[t], TM_ME);
164 }
165
166
167         // 4. The Bit Masking Process
168         vector<bool> vertexAssigned(m_vertexCount, false);
169         vector<bool> edgeAssigned(m_edgeCount, false);
170         for (unsigned int t = 0; (t < m_triangleCount); ++t) 
171         {
172         for (unsigned char i = 0; i < 3; ++i) 
173         {
174         if (!vertexAssigned[m_triangles[t].getVertex(i)]) 
175         {
176         vertexAssigned[m_triangles[t].getVertex(i)] = true;
177         setTriangleMark(m_triangleMarks[t], 1 << (2 + i));
178 }
179         if (!edgeAssigned[m_triangles[t].getEdge(i)]) 
180         {
181         edgeAssigned[m_triangles[t].getEdge(i)] = true;
182         setTriangleMark(m_triangleMarks[t], 1 << (5 + i));
183 }
184 }
185 }
186         */
187 }
188
189 // w3 is not perfect
190 void bvh_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3)
191 {
192         double  tempV1[3], tempV2[3], tempV4[3];
193         double  a,b,c,d,e,f;
194
195         VECSUB (tempV1, p1, p3);        
196         VECSUB (tempV2, p2, p3);        
197         VECSUB (tempV4, pv, p3);        
198         
199         a = INPR (tempV1, tempV1);      
200         b = INPR (tempV1, tempV2);      
201         c = INPR (tempV2, tempV2);      
202         e = INPR (tempV1, tempV4);      
203         f = INPR (tempV2, tempV4);      
204         
205         d = (a * c - b * b);
206         
207         if (ABS(d) < ALMOST_ZERO) {
208                 *w1 = *w2 = *w3 = 1.0 / 3.0;
209                 return;
210         }
211         
212         w1[0] = (float)((e * c - b * f) / d);
213         
214         if(w1[0] < 0)
215                 w1[0] = 0;
216         
217         w2[0] = (float)((f - b * (double)w1[0]) / c);
218         
219         if(w2[0] < 0)
220                 w2[0] = 0;
221         
222         w3[0] = 1.0f - w1[0] - w2[0];
223 }
224
225 DO_INLINE void interpolateOnTriangle(float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3) 
226 {
227         to[0] = to[1] = to[2] = 0;
228         VECADDMUL(to, v1, w1);
229         VECADDMUL(to, v2, w2);
230         VECADDMUL(to, v3, w3);
231 }
232
233
234 DO_INLINE void calculateFrictionImpulse(float to[3], float vrel[3], float normal[3], double normalVelocity,
235                                         double frictionConstant, double delta_V_n) 
236 {
237         float vrel_t_pre[3];
238         float vrel_t[3];
239         VECSUBS(vrel_t_pre, vrel, normal, normalVelocity);
240         VECCOPY(to, vrel_t_pre);
241         VecMulf(to, MAX2(1.0f - frictionConstant * delta_V_n / INPR(vrel_t_pre,vrel_t_pre), 0.0f));
242 }
243
244                 
245 int collision_static(ClothModifierData *clmd, ClothModifierData *coll_clmd)
246 {
247         unsigned int i = 0;
248         int result = 0;
249         LinkNode *search = NULL;
250         CollPair *collpair = NULL;
251         Cloth *cloth1, *cloth2;
252         float w1, w2, w3, u1, u2, u3;
253         float v1[3], v2[3], relativeVelocity[3];
254         float magrelVel;
255         
256         cloth1 = clmd->clothObject;
257         cloth2 = coll_clmd->clothObject;
258
259         search = clmd->coll_parms.collision_list;
260         
261         while(search)
262         {
263                 collpair = search->link;
264                 
265                 // compute barycentric coordinates for both collision points
266                 bvh_compute_barycentric(collpair->pa,
267                                         cloth1->verts[collpair->ap1].txold,
268      cloth1->verts[collpair->ap2].txold,
269      cloth1->verts[collpair->ap3].txold, 
270      &w1, &w2, &w3);
271         
272                 bvh_compute_barycentric(collpair->pb,
273                                         cloth2->verts[collpair->bp1].txold,
274      cloth2->verts[collpair->bp2].txold,
275      cloth2->verts[collpair->bp3].txold,
276      &u1, &u2, &u3);
277         
278                 // Calculate relative "velocity".
279                 interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
280                 
281                 interpolateOnTriangle(v2, cloth2->verts[collpair->bp1].tv, cloth2->verts[collpair->bp2].tv, cloth2->verts[collpair->bp3].tv, u1, u2, u3);
282                 
283                 VECSUB(relativeVelocity, v1, v2);
284                         
285                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
286                 magrelVel = INPR(relativeVelocity, collpair->normal);
287                 
288                 // printf("magrelVel: %f\n", magrelVel);
289                                 
290                 // Calculate masses of points.
291                 
292                 // If v_n_mag < 0 the edges are approaching each other.
293                 if(magrelVel < -ALMOST_ZERO) 
294                 {
295                         // Calculate Impulse magnitude to stop all motion in normal direction.
296                         // const double I_mag = v_n_mag / (1/m1 + 1/m2);
297                         float magnitude_i = magrelVel / 2.0f; // TODO implement masses
298                         float tangential[3], magtangent, magnormal, collvel[3];
299                         float vrel_t_pre[3];
300                         float vrel_t[3];
301                         double impulse;
302                         float epsilon = clmd->coll_parms.epsilon;
303                         float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
304                         
305                         // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms.friction*0.01, magrelVel);
306                         
307                         // magtangent = INPR(tangential, tangential);
308                         
309                         // Apply friction impulse.
310                         if (magtangent < -ALMOST_ZERO) 
311                         {
312                                 
313                                 // printf("friction applied: %f\n", magtangent);
314                                 // TODO check original code 
315                                 /*
316                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,tangential);
317                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv,tangential);
318                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv,tangential);
319                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v4].tv,tangential);
320                                 */
321                         }
322                         
323
324                         impulse = -2.0f * magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
325                         
326                         // printf("impulse: %f\n", impulse);
327                         
328                         VECADDMUL(cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse); 
329                         cloth1->verts[collpair->ap1].impulse_count++;
330                         
331                         VECADDMUL(cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse); 
332                         cloth1->verts[collpair->ap2].impulse_count++;
333                         
334                         VECADDMUL(cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse); 
335                         cloth1->verts[collpair->ap3].impulse_count++;
336                         
337                         result = 1;
338                         
339                         /*
340                         if (overlap > ALMOST_ZERO) {
341                         double I_mag  = overlap * 0.1;
342                                 
343                         impulse = -I_mag / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
344                                 
345                         VECADDMUL(cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse); 
346                         cloth1->verts[collpair->ap1].impulse_count++;
347                                                         
348                         VECADDMUL(cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse); 
349                         cloth1->verts[collpair->ap2].impulse_count++;
350                         
351                         VECADDMUL(cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse); 
352                         cloth1->verts[collpair->ap3].impulse_count++;
353                 }
354                         */
355                 
356                         // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
357                         
358                         // Apply the impulse and increase impulse counters.
359
360                         /*                      
361                         // calculateFrictionImpulse(tangential, collvel, collpair->normal, magtangent, clmd->coll_parms.friction*0.01, magtangent);
362                         VECSUBS(vrel_t_pre, collvel, collpair->normal, magnormal);
363                         // VecMulf(vrel_t_pre, clmd->coll_parms.friction*0.01f/INPR(vrel_t_pre,vrel_t_pre));
364                         magtangent = Normalize(vrel_t_pre);
365                         VecMulf(vrel_t_pre, MIN2(clmd->coll_parms.friction*0.01f*magnormal,magtangent));
366                         
367                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,vrel_t_pre);
368                         */
369                         
370                         
371                         
372                 }
373                 
374                 search = search->next;
375         }
376         
377                 
378         return result;
379 }
380
381 void bvh_collision_response_static(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree *tree1, Tree *tree2)
382 {
383         CollPair *collpair = NULL;
384         Cloth *cloth1=NULL, *cloth2=NULL;
385         MFace *face1=NULL, *face2=NULL;
386         ClothVertex *verts1=NULL, *verts2=NULL;
387         double distance = 0;
388         float epsilon = clmd->coll_parms.epsilon;
389         unsigned int i = 0;
390
391         for(i = 0; i < 4; i++)
392         {
393                 collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");                
394                 
395                 cloth1 = clmd->clothObject;
396                 cloth2 = coll_clmd->clothObject;
397                 
398                 verts1 = cloth1->verts;
399                 verts2 = cloth2->verts;
400         
401                 face1 = &(cloth1->mfaces[tree1->tri_index]);
402                 face2 = &(cloth2->mfaces[tree2->tri_index]);
403                 
404                 // check all possible pairs of triangles
405                 if(i == 0)
406                 {
407                         collpair->ap1 = face1->v1;
408                         collpair->ap2 = face1->v2;
409                         collpair->ap3 = face1->v3;
410                         
411                         collpair->bp1 = face2->v1;
412                         collpair->bp2 = face2->v2;
413                         collpair->bp3 = face2->v3;
414                         
415                 }
416                 
417                 if(i == 1)
418                 {
419                         if(face1->v4)
420                         {
421                                 collpair->ap1 = face1->v3;
422                                 collpair->ap2 = face1->v4;
423                                 collpair->ap3 = face1->v1;
424                                 
425                                 collpair->bp1 = face2->v1;
426                                 collpair->bp2 = face2->v2;
427                                 collpair->bp3 = face2->v3;
428                         }
429                         else
430                                 i++;
431                 }
432                 
433                 if(i == 2)
434                 {
435                         if(face2->v4)
436                         {
437                                 collpair->ap1 = face1->v1;
438                                 collpair->ap2 = face1->v2;
439                                 collpair->ap3 = face1->v3;
440                                 
441                                 collpair->bp1 = face2->v3;
442                                 collpair->bp2 = face2->v4;
443                                 collpair->bp3 = face2->v1;
444                         }
445                         else
446                                 i+=2;
447                 }
448                 
449                 if(i == 3)
450                 {
451                         if((face1->v4)&&(face2->v4))
452                         {
453                                 collpair->ap1 = face1->v3;
454                                 collpair->ap2 = face1->v4;
455                                 collpair->ap3 = face1->v1;
456                                 
457                                 collpair->bp1 = face2->v3;
458                                 collpair->bp2 = face2->v4;
459                                 collpair->bp3 = face2->v1;
460                         }
461                         else
462                                 i++;
463                 }
464                 
465                 // calc SIPcode (?)
466                 
467                 if(i < 4)
468                 {
469                         // calc distance + normal       
470                         distance = plNearestPoints(
471                                         verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, verts2[collpair->bp1].txold, verts2[collpair->bp2].txold, verts2[collpair->bp3].txold, 
472      collpair->pa, collpair->pb, collpair->vector);
473                         
474                         if (distance <= (epsilon + ALMOST_ZERO))
475                         {
476                                 // printf("dist: %f\n", (float)distance);
477                                 
478                                 // collpair->face1 = tree1->tri_index;
479                                 // collpair->face2 = tree2->tri_index;
480                                 
481                                 VECCOPY(collpair->normal, collpair->vector);
482                                 Normalize(collpair->normal);
483                                 
484                                 collpair->distance = distance;
485                                 BLI_linklist_append(&clmd->coll_parms.collision_list, collpair);
486                         }
487                         else
488                         {
489                                 MEM_freeN(collpair);
490                         }
491                 }
492                 else
493                 {
494                         MEM_freeN(collpair);
495                 }
496         }
497 }
498
499 void bvh_collision_response_moving(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree *tree1, Tree *tree2)
500 {
501         CollPair *collpair = NULL;
502         Cloth *cloth1=NULL, *cloth2=NULL;
503         MFace *face1=NULL, *face2=NULL;
504         ClothVertex *verts1=NULL, *verts2=NULL;
505         double distance = 0;
506         float epsilon = clmd->coll_parms.epsilon;
507         unsigned int i = 0;
508
509         for(i = 0; i < 4; i++)
510         {
511                 collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");                
512                 
513                 cloth1 = clmd->clothObject;
514                 cloth2 = coll_clmd->clothObject;
515                 
516                 verts1 = cloth1->verts;
517                 verts2 = cloth2->verts;
518         
519                 face1 = &(cloth1->mfaces[tree1->tri_index]);
520                 face2 = &(cloth2->mfaces[tree2->tri_index]);
521                 
522                 // check all possible pairs of triangles
523                 if(i == 0)
524                 {
525                         collpair->ap1 = face1->v1;
526                         collpair->ap2 = face1->v2;
527                         collpair->ap3 = face1->v3;
528                         
529                         collpair->bp1 = face2->v1;
530                         collpair->bp2 = face2->v2;
531                         collpair->bp3 = face2->v3;
532                         
533                 }
534                 
535                 if(i == 1)
536                 {
537                         if(face1->v4)
538                         {
539                                 collpair->ap1 = face1->v3;
540                                 collpair->ap2 = face1->v4;
541                                 collpair->ap3 = face1->v1;
542                                 
543                                 collpair->bp1 = face2->v1;
544                                 collpair->bp2 = face2->v2;
545                                 collpair->bp3 = face2->v3;
546                         }
547                         else
548                                 i++;
549                 }
550                 
551                 if(i == 2)
552                 {
553                         if(face2->v4)
554                         {
555                                 collpair->ap1 = face1->v1;
556                                 collpair->ap2 = face1->v2;
557                                 collpair->ap3 = face1->v3;
558                                 
559                                 collpair->bp1 = face2->v3;
560                                 collpair->bp2 = face2->v4;
561                                 collpair->bp3 = face2->v1;
562                         }
563                         else
564                                 i+=2;
565                 }
566                 
567                 if(i == 3)
568                 {
569                         if((face1->v4)&&(face2->v4))
570                         {
571                                 collpair->ap1 = face1->v3;
572                                 collpair->ap2 = face1->v4;
573                                 collpair->ap3 = face1->v1;
574                                 
575                                 collpair->bp1 = face2->v3;
576                                 collpair->bp2 = face2->v4;
577                                 collpair->bp3 = face2->v1;
578                         }
579                         else
580                                 i++;
581                 }
582                 
583                 // calc SIPcode (?)
584                 
585                 if(i < 4)
586                 {
587                         // calc distance + normal       
588                         distance = plNearestPoints(
589                                         verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, verts2[collpair->bp1].txold, verts2[collpair->bp2].txold, verts2[collpair->bp3].txold, 
590      collpair->pa, collpair->pb, collpair->vector);
591                         
592                         if (distance <= (epsilon + ALMOST_ZERO))
593                         {
594                                 // printf("dist: %f\n", (float)distance);
595                                 
596                                 // collpair->face1 = tree1->tri_index;
597                                 // collpair->face2 = tree2->tri_index;
598                                 
599                                 VECCOPY(collpair->normal, collpair->vector);
600                                 Normalize(collpair->normal);
601                                 
602                                 collpair->distance = distance;
603                                 BLI_linklist_append(&clmd->coll_parms.collision_list, collpair);
604                         }
605                         else
606                         {
607                                 MEM_freeN(collpair);
608                         }
609                 }
610                 else
611                 {
612                         MEM_freeN(collpair);
613                 }
614         }
615 }
616
617 // move collision objects forward in time and update static bounding boxes
618 void cloth_update_collision_objects(float step)
619 {
620         Base *base=NULL;
621         ClothModifierData *coll_clmd=NULL;
622         Object *coll_ob=NULL;
623         unsigned int i=0;
624         
625         // search all objects for collision object
626         for (base = G.scene->base.first; base; base = base->next)
627         {
628
629                 coll_ob = base->object;
630                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
631                 if (!coll_clmd)
632                         continue;
633
634                 // if collision object go on
635                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
636                 {
637                         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
638                         {
639                                 Cloth *coll_cloth = coll_clmd->clothObject;
640                                 BVH *coll_bvh = coll_clmd->clothObject->tree;
641                                 unsigned int coll_numverts = coll_cloth->numverts;
642
643                                 // update position of collision object
644                                 for(i = 0; i < coll_numverts; i++)
645                                 {
646                                         VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
647
648                                         VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
649                                         
650                                         // no dt here because of float rounding errors
651                                         VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
652                                 }
653                                 
654                                 // update BVH of collision object
655                                 bvh_update(coll_clmd, coll_bvh, 0); // 0 means STATIC, 1 means MOVING 
656                         }
657                         else
658                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
659                 }
660         }
661 }
662
663 // CLOTH_MAX_THRESHOLD defines how much collision rounds/loops should be taken
664 #define CLOTH_MAX_THRESHOLD 10
665
666 // cloth - object collisions
667 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, float dt)
668 {
669         Base *base=NULL;
670         ClothModifierData *coll_clmd=NULL;
671         Cloth *cloth=NULL;
672         Object *coll_ob=NULL;
673         BVH *cloth_bvh=NULL;
674         unsigned int i=0, j = 0, numfaces = 0, numverts = 0;
675         unsigned int result = 0, ic = 0, rounds = 0; // result counts applied collisions; ic is for debug output; 
676         ClothVertex *verts = NULL;
677         float tnull[3] = {0,0,0};
678         int ret = 0;
679
680         if ((clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
681         {
682                 return 0;
683         }
684         cloth = clmd->clothObject;
685         verts = cloth->verts;
686         cloth_bvh = (BVH *) cloth->tree;
687         numfaces = clmd->clothObject->numfaces;
688         numverts = clmd->clothObject->numverts;
689         
690         ////////////////////////////////////////////////////////////
691         // static collisions
692         ////////////////////////////////////////////////////////////
693
694         // update cloth bvh
695         bvh_update(clmd, cloth_bvh, 0); // 0 means STATIC, 1 means MOVING (see later in this function)
696         
697         // update collision objects
698         cloth_update_collision_objects(step);
699         
700         do
701         {
702                 result = 0;
703                 ic = 0;
704                 clmd->coll_parms.collision_list = NULL; 
705                 
706                 // check all collision objects
707                 for (base = G.scene->base.first; base; base = base->next)
708                 {
709                         coll_ob = base->object;
710                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
711                         
712                         if (!coll_clmd)
713                                 continue;
714                         
715                         // if collision object go on
716                         if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
717                         {
718                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
719                                 {
720                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
721                                         
722                                         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, bvh_collision_response_static);
723                                 }
724                                 else
725                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
726                         }
727                 }
728                 
729                 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
730                 result = 1;
731                 for(j = 0; j < 50; j++) // 50 is just a value that ensures convergence
732                 {
733                         result = 0;
734                         
735                         // handle all collision objects
736                         for (base = G.scene->base.first; base; base = base->next)
737                         {
738                 
739                                 coll_ob = base->object;
740                                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
741                                 if (!coll_clmd)
742                                         continue;
743                 
744                                 // if collision object go on
745                                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
746                                 {
747                                         if (coll_clmd->clothObject) 
748                                                 result += collision_static(clmd, coll_clmd);
749                                         else
750                                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
751                                 }
752                         }
753                         
754                         // apply impulses in parallel
755                         ic=0;
756                         for(i = 0; i < numverts; i++)
757                         {
758                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
759                                 if(verts[i].impulse_count)
760                                 {
761                                         VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
762                                         VECCOPY(verts[i].impulse, tnull);
763                                         verts[i].impulse_count = 0;
764                                 
765                                         ic++;
766                                         ret++;
767                                 }
768                         }
769                 }
770                 
771                 // free collision list
772                 if(clmd->coll_parms.collision_list)
773                 {
774                         LinkNode *search = clmd->coll_parms.collision_list;
775                         while(search)
776                         {
777                                 CollPair *coll_pair = search->link;
778                                                         
779                                 MEM_freeN(coll_pair);
780                                 search = search->next;
781                         }
782                         BLI_linklist_free(clmd->coll_parms.collision_list,NULL);
783                         
784                         clmd->coll_parms.collision_list = NULL;
785                 }
786                 
787                 printf("ic: %d\n", ic);
788                 rounds++;
789         }
790         while(result && (CLOTH_MAX_THRESHOLD>rounds));
791         
792         printf("\n");
793                         
794         ////////////////////////////////////////////////////////////
795         // update positions
796         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
797         ////////////////////////////////////////////////////////////
798         
799         // verts come from clmd
800         for(i = 0; i < numverts; i++)
801         {
802                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
803         }
804         ////////////////////////////////////////////////////////////
805
806         ////////////////////////////////////////////////////////////
807         // moving collisions
808         ////////////////////////////////////////////////////////////
809
810         
811         // update cloth bvh
812         bvh_update(clmd, cloth_bvh, 1);  // 0 means STATIC, 1 means MOVING 
813         
814         // update moving bvh for collision object once
815         for (base = G.scene->base.first; base; base = base->next)
816         {
817                 
818                 coll_ob = base->object;
819                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
820                 if (!coll_clmd)
821                         continue;
822                 
823                 if(!coll_clmd->clothObject)
824                         continue;
825                 
826                                 // if collision object go on
827                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
828                 {
829                         BVH *coll_bvh = coll_clmd->clothObject->tree;
830                         
831                         bvh_update(coll_clmd, coll_bvh, 1);  // 0 means STATIC, 1 means MOVING  
832                 }
833         }
834         
835         
836         do
837         {
838                 result = 0;
839                 ic = 0;
840                 clmd->coll_parms.collision_list = NULL; 
841                 
842                 // check all collision objects
843                 for (base = G.scene->base.first; base; base = base->next)
844                 {
845                         coll_ob = base->object;
846                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
847                         
848                         if (!coll_clmd)
849                                 continue;
850                         
851                         // if collision object go on
852                         if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
853                         {
854                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
855                                 {
856                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
857                                         
858                                         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, bvh_collision_response_moving);
859                                 }
860                                 else
861                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
862                         }
863                 }
864                 /*
865                 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
866                 result = 1;
867                 for(j = 0; j < 50; j++) // 50 is just a value that ensures convergence
868                 {
869                 result = 0;
870                         
871                         // handle all collision objects
872                 for (base = G.scene->base.first; base; base = base->next)
873                 {
874                 
875                 coll_ob = base->object;
876                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
877                                 
878                 if (!coll_clmd)
879                 continue;
880                 
881                                 // if collision object go on
882                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
883                 {
884                 if (coll_clmd->clothObject) 
885                 result += collision_moving(clmd, coll_clmd);
886                 else
887                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
888         }
889         }
890                         
891                         // apply impulses in parallel
892                 ic=0;
893                 for(i = 0; i < numverts; i++)
894                 {
895                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
896                 if(verts[i].impulse_count)
897                 {
898                 VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
899                 VECCOPY(verts[i].impulse, tnull);
900                 verts[i].impulse_count = 0;
901                                 
902                 ic++;
903                 ret++;
904         }
905         }
906         }
907                 */
908                 
909                 // verts come from clmd
910                 for(i = 0; i < numverts; i++)
911                 {
912                         VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
913                 }
914                 
915                 // update cloth bvh
916                 bvh_update(clmd, cloth_bvh, 1);  // 0 means STATIC, 1 means MOVING 
917                 
918                 
919                 // free collision list
920                 if(clmd->coll_parms.collision_list)
921                 {
922                         LinkNode *search = clmd->coll_parms.collision_list;
923                         while(search)
924                         {
925                                 CollPair *coll_pair = search->link;
926                                                         
927                                 MEM_freeN(coll_pair);
928                                 search = search->next;
929                         }
930                         BLI_linklist_free(clmd->coll_parms.collision_list,NULL);
931                         
932                         clmd->coll_parms.collision_list = NULL;
933                 }
934                 
935                 printf("ic: %d\n", ic);
936                 rounds++;
937         }
938         while(result && (CLOTH_MAX_THRESHOLD>rounds));
939         
940         
941         ////////////////////////////////////////////////////////////
942         // update positions + velocities
943         ////////////////////////////////////////////////////////////
944         
945         // verts come from clmd
946         for(i = 0; i < numverts; i++)
947         {
948                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
949         }
950         ////////////////////////////////////////////////////////////
951
952         return MIN2(ret, 1);
953 }