Fixed collision object init, New: Apply collision impulses so minimum distance is...
[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
190 void bvh_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3], double *w1, double *w2, double *w3)
191 {
192         float   tempV1[3], tempV2[3], tempV4[3];
193         double  a,b,c,e,f;
194
195         VECSUB (tempV1, p1, p3);        /* x1 - x3 */
196         VECSUB (tempV2, p2, p3);        /* x2 - x3 */
197         VECSUB (tempV4, pv, p3);        /* pv - x3 */
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         
206         w1[0] = (e * c - b * f) / (a * c - b * b);
207         w2[0] = (f - b * w1[0]) / c;
208         w3[0] = 1.0 - w1[0] - w2[0];
209 }
210
211 DO_INLINE void interpolateOnTriangle(float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3) 
212 {
213         to[0] = to[1] = to[2] = 0;
214         VECADDMUL(to, v1, w1);
215         VECADDMUL(to, v2, w2);
216         VECADDMUL(to, v3, w3);
217 }
218
219
220 DO_INLINE void calculateFrictionImpulse(float to[3], float vrel[3], float normal[3], double normalVelocity,
221         double frictionConstant, double delta_V_n) 
222 {
223         float vrel_t_pre[3];
224         float vrel_t[3];
225         VECSUBS(vrel_t_pre, vrel, normal, normalVelocity);
226         VECCOPY(to, vrel_t_pre);
227         VecMulf(to, MAX2(1.0f - frictionConstant * delta_V_n / INPR(vrel_t_pre,vrel_t_pre), 0.0f));
228 }
229
230                 
231 int collision_static(ClothModifierData *clmd, ClothModifierData *coll_clmd, LinkNode **collision_list)
232 {
233         unsigned int i = 0, numverts=0;
234         int result = 0;
235         LinkNode *search = NULL;
236         CollPair *collpair = NULL;
237         Cloth *cloth1, *cloth2;
238         MFace *face1, *face2;
239         double w1, w2, w3, u1, u2, u3;
240         float v1[3], v2[3], relativeVelocity[3];
241         float magrelVel;
242         
243         cloth1 = clmd->clothObject;
244         cloth2 = coll_clmd->clothObject;
245         
246         numverts = clmd->clothObject->numverts;
247         
248         /*
249         for(i = 0; i < LIST_LENGTH; i++)
250         {
251                 // calc SIP-code
252                 //  TODO for later: calculateSipCode()
253         
254                 // calc distance (?)
255                 
256                 // calc impulse
257                 
258                 // apply impulse
259         }
260         */
261         
262         for(i = 0; i < numverts; i++)
263         {
264                 search = collision_list[i];
265                 
266                 while(search)
267                 {
268                         collpair = search->link;
269                         
270                         face1 = &(cloth1->mfaces[collpair->face1]);
271                         face2 = &(cloth2->mfaces[collpair->face2]);
272                         
273                         // compute barycentric coordinates for both collision points
274                         
275                         if(!collpair->quadA)
276                                 bvh_compute_barycentric(collpair->p1,
277                                                         cloth1->verts[face1->v1].txold,
278                                                         cloth1->verts[face1->v2].txold,
279                                                         cloth1->verts[face1->v3].txold, 
280                                                         &w1, &w2, &w3);
281                         else
282                                 bvh_compute_barycentric(collpair->p1,
283                                                         cloth1->verts[face1->v4].txold,
284                                                         cloth1->verts[face1->v1].txold,
285                                                         cloth1->verts[face1->v3].txold, 
286                                                         &w1, &w2, &w3);
287                         
288                         if(!collpair->quadB)
289                                 bvh_compute_barycentric(collpair->p2,
290                                                         cloth2->verts[face2->v1].txold,
291                                                         cloth2->verts[face2->v2].txold,
292                                                         cloth2->verts[face2->v3].txold, 
293                                                         &u1, &u2, &u3);
294                         else
295                                 bvh_compute_barycentric(collpair->p2,
296                                                         cloth2->verts[face2->v4].txold,
297                                                         cloth2->verts[face2->v1].txold,
298                                                         cloth2->verts[face2->v3].txold, 
299                                                         &u1, &u2, &u3);
300                         
301                         // Calculate relative "velocity".
302                         
303                         if(!collpair->quadA)
304                                 interpolateOnTriangle(v1, cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv, cloth1->verts[face1->v3].tv, w1, w2, w3);
305                         else
306                                 interpolateOnTriangle(v1, cloth1->verts[face1->v4].tv, cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv, w1, w2, w3);
307                         
308                         if(!collpair->quadB)
309                                 interpolateOnTriangle(v2, cloth2->verts[face2->v1].tv, cloth2->verts[face2->v2].tv, cloth2->verts[face2->v3].tv, u1, u2, u3);
310                         else
311                                 interpolateOnTriangle(v2, cloth2->verts[face2->v4].tv, cloth2->verts[face2->v1].tv, cloth2->verts[face2->v3].tv, u1, u2, u3);
312                         
313                         VECSUB(relativeVelocity, v1, v2);
314                                 
315                         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
316                         magrelVel = INPR(relativeVelocity, collpair->normal);
317                                         
318                         // Calculate masses of points.
319                         
320                         // printf("relativeVelocity -> x: %f, y: %f, z: %f\n", relativeVelocity[0], relativeVelocity[1],relativeVelocity[2]); 
321                         
322                         // If v_n_mag > 0 the edges are approaching each other.
323                         if(magrelVel > ALMOST_ZERO)
324                         {
325                                 // Calculate Impulse magnitude to stop all motion in normal direction.
326                                 // const double I_mag = v_n_mag / (1/m1 + 1/m2);
327                                 float magnitude_i = magrelVel / 2.0f; // TODO implement masses
328                                 float tangential[3], magtangent, magnormal, collvel[3];
329                                 float vrel_t_pre[3];
330                                 float vrel_t[3], impulse;
331                                 float epsilon = clmd->coll_parms.epsilon;
332                                 
333                                 // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms.friction*0.01, magrelVel);
334                                 
335                                 // magtangent = INPR(tangential, tangential);
336                                 
337                                 // Apply friction impulse.
338                                 if (magtangent > ALMOST_ZERO) 
339                                 {
340                                         
341                                         // printf("friction applied: %f\n", magtangent);
342                                         // TODO check original code 
343                                         /*
344                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,tangential);
345                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv,tangential);
346                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv,tangential);
347                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v4].tv,tangential);
348                                         */
349                                 }
350                         
351                                 // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
352                                 
353                                 // Apply the impulse and increase impulse counters.
354                                 // my try, works better than the papers ones (maybe i did just something wrong)
355                                 VECSUB(collvel, cloth1->verts[face1->v1].tv, v2);
356                                 magnormal = INPR(collvel, collpair->normal);
357                                 if(magnormal<ALMOST_ZERO)
358                                         magnormal = 0.0f;
359                                 
360                                 impulse = (epsilon + ALMOST_ZERO-collpair->distance) / 4.0f;
361                                 VECADDMUL(cloth1->verts[face1->v1].tv, collpair->normal, -magnormal + impulse); 
362                                 
363                                 // calculateFrictionImpulse(tangential, collvel, collpair->normal, magtangent, clmd->coll_parms.friction*0.01, magtangent);
364                                 
365 /*
366                                 VECSUBS(vrel_t_pre, collvel, collpair->normal, magnormal);
367                                 // VecMulf(vrel_t_pre, clmd->coll_parms.friction*0.01f/INPR(vrel_t_pre,vrel_t_pre));
368                                 magtangent = Normalize(vrel_t_pre);
369                                 VecMulf(vrel_t_pre, MIN2(clmd->coll_parms.friction*0.01f*magnormal,magtangent));
370                                 
371                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,vrel_t_pre);
372 */                              
373                                 
374                                 VECSUB(collvel, cloth1->verts[face1->v2].tv, v2);
375                                 magnormal = INPR(collvel, collpair->normal);
376                                 if(magnormal<ALMOST_ZERO)
377                                         magnormal = 0.0f;
378                                 
379                                 impulse = (epsilon + ALMOST_ZERO-collpair->distance) / 4.0f;
380                                 VECADDMUL(cloth1->verts[face1->v2].tv, collpair->normal, -magnormal+ impulse); 
381                                 
382                                 
383                                 VECSUB(collvel, cloth1->verts[face1->v3].tv, v2);
384                                 magnormal = INPR(collvel, collpair->normal);
385                                 if(magnormal<ALMOST_ZERO)
386                                         magnormal = 0.0f;
387                                 
388                                 impulse = (epsilon + ALMOST_ZERO-collpair->distance) / 4.0f;
389                                 VECADDMUL(cloth1->verts[face1->v3].tv, collpair->normal, -magnormal+ impulse); 
390                                 
391                                 
392                                 VECSUB(collvel, cloth1->verts[face1->v4].tv, v2);
393                                 magnormal = INPR(collvel, collpair->normal);
394                                 if(magnormal<ALMOST_ZERO)
395                                         magnormal = 0.0f;
396                                 
397                                 impulse = (epsilon + ALMOST_ZERO-collpair->distance) / 4.0f;
398                                 VECADDMUL(cloth1->verts[face1->v4].tv, collpair->normal, -magnormal+ impulse); 
399                         
400                                 result = 1;
401                                 
402                         }
403                         
404                         search = search->next;
405                 }
406         }
407                 
408         return result;
409 }
410
411 // return distance between two triangles using bullet engine
412 double implicit_tri_check_coherence (ClothModifierData *clmd, ClothModifierData *coll_clmd, unsigned int tri_index1, unsigned int tri_index2, float pa[3], float pb[3], float normal[3], int quadA, int quadB)
413 {
414         MFace *face1=NULL, *face2=NULL;
415         float  a[3][3];
416         float  b[3][3];
417         double distance=0, tempdistance=0;
418         Cloth *cloth1=NULL, *cloth2=NULL;
419         float tpa[3], tpb[3], tnormal[3];
420         unsigned int indexA=0, indexB=0, indexC=0, indexD=0, indexE=0, indexF=0;
421         int i = 0;
422         
423         cloth1 = clmd->clothObject;
424         cloth2 = coll_clmd->clothObject;
425         
426         face1 = &(cloth1->mfaces[tri_index1]);
427         face2 = &(cloth2->mfaces[tri_index2]);
428         
429         // face a1 + face b1
430         VECCOPY(a[0], cloth1->verts[face1->v1].txold);
431         VECCOPY(a[1], cloth1->verts[face1->v2].txold);
432         VECCOPY(a[2], cloth1->verts[face1->v3].txold);
433         
434         
435         VECCOPY(b[0], cloth2->verts[face2->v1].txold);
436         VECCOPY(b[1], cloth2->verts[face2->v2].txold);
437         VECCOPY(b[2], cloth2->verts[face2->v3].txold);
438
439         distance = plNearestPoints(a,b,pa,pb,normal);
440         
441         quadA = quadB = 0;
442         
443         for(i = 0; i < 3; i++)
444         {
445                 if(i == 0)
446                 {
447                         indexA = face1->v4;
448                         indexB = face1->v1;
449                         indexC = face1->v3;
450                         
451                         indexD = face2->v1;
452                         indexE = face2->v2;
453                         indexF = face2->v3;
454                 }
455                 else if(i == 1)
456                 {
457                         indexA = face1->v4;
458                         indexB = face1->v1;
459                         indexC = face1->v3;
460                 
461                         indexD = face2->v4;
462                         indexE = face2->v1;
463                         indexF = face2->v3;
464                 }
465                 else if(i == 2)
466                 {
467                         indexA = face1->v1;
468                         indexB = face1->v2;
469                         indexC = face1->v3;
470                 
471                         indexD = face2->v4;
472                         indexE = face2->v1;
473                         indexF = face2->v3;
474                 }
475                 
476                 // face a2 + face b1
477                 VECCOPY(a[0], cloth1->verts[indexA].txold);
478                 VECCOPY(a[1], cloth1->verts[indexB].txold);
479                 VECCOPY(a[2], cloth1->verts[indexC].txold);
480                 
481                 
482                 VECCOPY(b[0], cloth2->verts[indexD].txold);
483                 VECCOPY(b[1], cloth2->verts[indexE].txold);
484                 VECCOPY(b[2], cloth2->verts[indexF].txold);
485
486                 tempdistance = plNearestPoints(a,b,tpa,tpb,tnormal);
487                 
488                 if(tempdistance < distance)
489                 {
490                         VECCOPY(pa, tpa);
491                         VECCOPY(pb, tpb);
492                         VECCOPY(normal, tnormal);
493                         distance = tempdistance;
494                         
495                         if(i == 0)
496                         {
497                                 quadA = 1; quadB = 0;
498                         }
499                         else if(i == 1)
500                         {
501                                 quadA = quadB = 1;
502                         }
503                         else if(i == 2)
504                         {
505                                 quadA = 0; quadB = 1;
506                         }
507                 }
508         }
509         return distance;
510 }
511
512 void bvh_collision_response(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree * tree1, Tree * tree2)
513 {
514         CollPair *collpair = NULL;
515         LinkNode **linknode;
516         double distance = 0;
517         float epsilon = clmd->coll_parms.epsilon;
518
519         collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");
520         linknode = clmd->coll_parms.temp;
521         
522         // calc SIPcode (?)
523         
524         // calc distance + normal       
525         distance = implicit_tri_check_coherence(clmd, coll_clmd, tree1->tri_index, tree2->tri_index, collpair->p1, collpair->p2, collpair->vector, collpair->quadA, collpair->quadB);
526         
527         if (ABS(distance) <= (epsilon + ALMOST_ZERO))
528         {
529                 // printf("distance: %f, epsilon: %f\n", (float)distance, epsilon + ALMOST_ZERO);
530                         
531                 collpair->face1 = tree1->tri_index;
532                 collpair->face2 = tree2->tri_index;
533                 
534                 VECCOPY(collpair->normal, collpair->vector);
535                 Normalize(collpair->normal);
536                 
537                 // printf("normal x: %f, y: %f, z: %f\n", collpair->normal[0], collpair->normal[1], collpair->normal[2]);
538                 
539                 collpair->distance = distance;
540                 BLI_linklist_append(&linknode[tree1->tri_index], collpair);     
541         }
542         else
543         {
544                 MEM_freeN(collpair);
545         }
546 }
547
548
549 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, CM_COLLISION_RESPONSE collision_response, float dt)
550 {
551         Base *base=NULL;
552         ClothModifierData *coll_clmd=NULL;
553         Cloth *cloth=NULL;
554         Object *coll_ob=NULL;
555         BVH *cloth_bvh=NULL;
556         unsigned int i=0, numverts=0;
557         int result = 0;
558
559         if ((clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
560         {
561                 return 0;
562         }
563         cloth = clmd->clothObject;
564         cloth_bvh = (BVH *) cloth->tree;
565         numverts = clmd->clothObject->numverts;
566         
567         ////////////////////////////////////////////////////////////
568         // static collisions
569         ////////////////////////////////////////////////////////////
570
571         // update cloth bvh
572         bvh_update_static(clmd, cloth_bvh);
573
574         // search all objects for collision object
575         for (base = G.scene->base.first; base; base = base->next)
576         {
577
578                 coll_ob = base->object;
579                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
580                 if (!coll_clmd)
581                         continue;
582
583                 // if collision object go on
584                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
585                 {
586                         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
587                         {                       
588                                 unsigned int coll_numverts = coll_clmd->clothObject->numverts;
589                                 Cloth *coll_cloth = coll_clmd->clothObject;
590
591                                 LinkNode **collision_list = MEM_callocN (sizeof(LinkNode *)*numverts, "collision_list");
592                                 BVH *coll_bvh = coll_clmd->clothObject->tree;
593
594                                 if(collision_list)
595                                 {                                       
596                                         // memset(collision_list, 0, sizeof(LinkNode *)*numverts); 
597                                         
598                                         for(i = 0; i < numverts; i++)
599                                         {
600                                                 collision_list[i] = NULL;
601                                         }
602
603                                         clmd->coll_parms.temp = collision_list;
604                                         
605                                         // update position of collision object
606                                         for(i = 0; i < coll_numverts; i++)
607                                         {
608                                                 VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
609
610                                                 VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
611                                                 
612                                                 VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
613                                         }
614                                                         
615                                         // update BVH of collision object
616                                         bvh_update_static(coll_clmd, coll_bvh);
617
618                                         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, collision_response);
619                                         
620                                         result += collision_static(clmd, coll_clmd, collision_list);
621                                         
622                                         // calculate velocities
623                                         
624                                         // free temporary list 
625                                         for(i = 0; i < numverts; i++)
626                                         {
627                                                 LinkNode *search = collision_list[i];
628                                                 while(search)
629                                                 {
630                                                         LinkNode *next= search->next;
631                                                         CollPair *collpair = search->link;
632                                                         
633                                                         if(collpair)
634                                                                 MEM_freeN(collpair);    
635
636                                                         search = next;
637                                                 }
638
639                                                 BLI_linklist_free(collision_list[i],NULL); 
640                                         }
641                                         if(collision_list)
642                                                 MEM_freeN(collision_list);
643
644                                         clmd->coll_parms.temp = NULL;
645                                 }
646                                 
647
648                         }
649                         else
650                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
651                 }
652         }
653
654         ////////////////////////////////////////////////////////////
655         // update positions + velocities
656         ////////////////////////////////////////////////////////////
657
658         // TODO 
659
660
661         ////////////////////////////////////////////////////////////
662         // moving collisions
663         ////////////////////////////////////////////////////////////
664
665         // TODO 
666         // bvh_update_moving(clmd, clmd->clothObject->tree);
667
668         return MIN2(result, 1);
669 }