WIP commit, (just in case my HD breaks down). Don't expect anything to work. Code...
[blender.git] / source / blender / blenkernel / intern / collision.c
index 8c6a17c4e425ca951f4f8c13f84cbeb2e564a6d0..f4b0ce7312b918372c30f6f3b0f65355770c2177 100644 (file)
@@ -49,6 +49,7 @@
 #include "BLI_arithb.h"
 #include "BLI_edgehash.h"
 #include "BLI_linklist.h"
+#include "BKE_collisions.h"
 #include "BKE_curve.h"
 #include "BKE_deform.h"
 #include "BKE_DerivedMesh.h"
 #include "Bullet-C-Api.h"
 
 
-#define DERANDOMIZE 1
+// step is limited from 0 (frame start position) to 1 (frame end position)
+void collision_move_object(CollisionModifierData *collmd, float step)
+{
+       float tv[3] = {0,0,0};
+       unsigned int i = 0;
+       MVert *tempVert = collmd->current_x;
+       collmd->current_x = collmd->current_xnew;
+       collmd->current_xnew = tempVert;
+                       
+       for ( i = 0; i < collmd->numverts; i++ )
+       {
+               VECSUB(tv, collmd->xnew[i].co, collmd->x[i].co);
+               VECADDS(collmd->current_xnew[i].co, collmd->x[i].co, tv, step);
+       }
+}
 
 
-enum TRIANGLE_MARK 
-{ 
-       TM_MV = 1,
-       TM_ME = 2,
-       TM_V1 = 4,
-       TM_V2 = 8,
-       TM_V3 = 16,
-       TM_E1 = 32,
-       TM_E2 = 64,
-       TM_E3 = 128 
-};
+/**
+ * gsl_poly_solve_cubic -
+ *
+ * copied from SOLVE_CUBIC.C --> GSL
+ */
+#define mySWAP(a,b) { float tmp = b ; b = a ; a = tmp ; }
 
-DO_INLINE int hasTriangleMark(unsigned char mark, unsigned char bit) { return mark & bit; }
-DO_INLINE void setTriangleMark(unsigned char *mark, unsigned char bit) { mark[0] |= bit; }
-DO_INLINE void clearTriangleMark(unsigned char *mark, unsigned char bit) { mark[0] &= ~bit; }
+int gsl_poly_solve_cubic (float a, float b, float c, float *x0, float *x1, float *x2)
+{
+       float q = (a * a - 3 * b);
+       float r = (2 * a * a * a - 9 * a * b + 27 * c);
 
+       float Q = q / 9;
+       float R = r / 54;
 
-void generateTriangleMarks() 
-{
-       /*
-       unsigned int firstEdge = 0;
-       
-       // 1. Initialization
-       memset(m_triangleMarks, 0, sizeof(unsigned char) * m_triangleCount);
+       float Q3 = Q * Q * Q;
+       float R2 = R * R;
 
-       // 2. The Marking Process
-       
-       // 2.1 Randomly mark triangles for covering vertices.
-       for (unsigned int v = 0; v < m_vertexCount; ++v) 
+       float CR2 = 729 * r * r;
+       float CQ3 = 2916 * q * q * q;
+
+       if (R == 0 && Q == 0)
        {
-               if (vertexCover(v) == 0) 
-               {
+               *x0 = - a / 3 ;
+               *x1 = - a / 3 ;
+               *x2 = - a / 3 ;
+               return 3 ;
+       }
+       else if (CR2 == CQ3) 
+       {
+         /* this test is actually R2 == Q3, written in a form suitable
+               for exact computation with integers */
 
-                       // Randomly select an edge whose first triangle we're going to flag. 
+         /* Due to finite precision some float roots may be missed, and
+               considered to be a pair of complex roots z = x +/- epsilon i
+               close to the real axis. */
 
-#ifndef DERANDOMIZE
-                       firstEdge = (unsigned int)((float)(random() & 0x7FFFFFFF) /
-                                       (float)(0x80000000) *
-                                       (float)(m_vertices[v].getEdgeCount()));
-#endif
-                       for (unsigned int ofs = 0; ofs < m_vertices[v].getEdgeCount(); ++ofs) 
-                       {
-                               unsigned int edgeIdx = (firstEdge + ofs) % m_vertices[v].getEdgeCount();
-                               if (m_edges[m_vertices[v].getEdge(edgeIdx)].getTriangleCount())
-                                       setTriangleMark(m_triangleMarks[m_edges[m_vertices[v].getEdge(edgeIdx)].getTriangle(0)], TM_MV);
-                       }
+               float sqrtQ = sqrtf (Q);
+
+               if (R > 0)
+               {
+                       *x0 = -2 * sqrtQ  - a / 3;
+                       *x1 = sqrtQ - a / 3;
+                       *x2 = sqrtQ - a / 3;
                }
+               else
+               {
+                       *x0 = - sqrtQ  - a / 3;
+                       *x1 = - sqrtQ - a / 3;
+                       *x2 = 2 * sqrtQ - a / 3;
+               }
+               return 3 ;
        }
-       */
-       /* If the Cloth is malformed (vertices without adjacent triangles) there might still be uncovered vertices. (Bad luck.) */
-       /*
-       // 2.2 Randomly mark triangles for covering edges.
-       for (unsigned int e = 0; e < m_edgeCount; ++e) 
+       else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
        {
-               if (m_edges[e].getTriangleCount() && (edgeCover(e) == 0)) 
+               float sqrtQ = sqrtf (Q);
+               float sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
+               float theta = acosf (R / sqrtQ3);
+               float norm = -2 * sqrtQ;
+               *x0 = norm * cosf (theta / 3) - a / 3;
+               *x1 = norm * cosf ((theta + 2.0 * M_PI) / 3) - a / 3;
+               *x2 = norm * cosf ((theta - 2.0 * M_PI) / 3) - a / 3;
+      
+               /* Sort *x0, *x1, *x2 into increasing order */
+
+               if (*x0 > *x1)
+                       mySWAP(*x0, *x1) ;
+      
+               if (*x1 > *x2)
                {
-#ifndef DERANDOMIZE
-                       setTriangleMark(m_triangleMarks[m_edges[e].getTriangle(static_cast<UINT32>((float)(random() & 0x7FFFFFFF) /
-                                       (float)(0x80000000) *
-                                       (float)(m_edges[e].getTriangleCount())))], TM_ME);
-#else
-                       setTriangleMark(m_triangleMarks[m_edges[e].getTriangle(0)], TM_ME);
-#endif
+                       mySWAP(*x1, *x2) ;
+          
+                       if (*x0 > *x1)
+                               mySWAP(*x0, *x1) ;
                }
+      
+               return 3;
        }
+       else
+       {
+               float sgnR = (R >= 0 ? 1 : -1);
+               float A = -sgnR * powf (fabs (R) + sqrtf (R2 - Q3), 1.0/3.0);
+               float B = Q / A ;
+               *x0 = A + B - a / 3;
+               return 1;
+       }
+}
 
-       
-       // 3. The Unmarking Process
-       for (unsigned int t = 0; (t < m_triangleCount); ++t) 
+
+/**
+ * gsl_poly_solve_quadratic
+ *
+ * copied from GSL
+ */
+int gsl_poly_solve_quadratic (float a, float b, float c,  float *x0, float *x1)
+{
+       float disc = b * b - 4 * a * c;
+
+       if (disc > 0)
        {
-               bool overCoveredVertices = true;
-               bool overCoveredEdges = true;
-               for (unsigned char i = 0; (i < 3) && (overCoveredVertices || overCoveredEdges); ++i) 
+               if (b == 0)
                {
+                       float r = fabs (0.5 * sqrtf (disc) / a);
+                       *x0 = -r;
+                       *x1 =  r;
+               }
+               else
+               {
+                       float sgnb = (b > 0 ? 1 : -1);
+                       float temp = -0.5 * (b + sgnb * sqrtf (disc));
+                       float r1 = temp / a ;
+                       float r2 = c / temp ;
 
-                       if (vertexCover(m_triangles[t].getVertex(i)) == 1)
-                               overCoveredVertices = false;
-                       if (edgeCover(m_triangles[t].getEdge(i)) == 1)
-                               overCoveredEdges = false;
-
-                       assert(vertexCover(m_triangles[t].getVertex(i)) > 0);
-                       assert(edgeCover(m_triangles[t].getEdge(i)) > 0);
+                       if (r1 < r2) 
+                       {
+                               *x0 = r1 ;
+                               *x1 = r2 ;
+                       } 
+                       else 
+                       {
+                               *x0 = r2 ;
+                               *x1 = r1 ;
+                       }
                }
-               if (overCoveredVertices)
-                       clearTriangleMark(m_triangleMarks[t], TM_MV);
-               if (overCoveredEdges)
-                       clearTriangleMark(m_triangleMarks[t], TM_ME);
+               return 2;
+       }
+       else if (disc == 0) 
+       {
+               *x0 = -0.5 * b / a ;
+               *x1 = -0.5 * b / a ;
+               return 2 ;
+       }
+       else
+       {
+               return 0;
        }
+}
+
+
+
+/*
+ * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
+ *     page 4, left column
+ */
 
+int collisions_get_collision_time(float a[3], float b[3], float c[3], float d[3], float e[3], float f[3], float solution[3]) 
+{
+       int num_sols = 0;
+       
+       float g = -a[2] * c[1] * e[0] + a[1] * c[2] * e[0] +
+                       a[2] * c[0] * e[1] - a[0] * c[2] * e[1] -
+                       a[1] * c[0] * e[2] + a[0] * c[1] * e[2];
+
+       float h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
+                       a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
+                       a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
+                       b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
+                       a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
+                       a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
+
+       float i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
+                       b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
+                       b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
+                       b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
+                       a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
+                       b[2] * c[0] * f[1] - b[0] * c[2] * f[1] + 
+                       a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
+                       b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
+                       a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
+
+       float j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
+                       b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
+                       b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
+
+       // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
+       if(ABS(j) > ALMOST_ZERO)
+       {
+               i /= j;
+               h /= j;
+               g /= j;
+               
+               num_sols = gsl_poly_solve_cubic(i, h, g, &solution[0], &solution[1], &solution[2]);
+       }
+       else if(ABS(i) > ALMOST_ZERO)
+       {       
+               num_sols = gsl_poly_solve_quadratic(i, h, g, &solution[0], &solution[1]);
+               solution[2] = -1.0;
+       }
+       else if(ABS(h) > ALMOST_ZERO)
+       {
+               solution[0] = -g / h;
+               solution[1] = solution[2] = -1.0;
+               num_sols = 1;
+       }
+       else if(ABS(g) > ALMOST_ZERO)
+       {
+               solution[0] = 0;
+               solution[1] = solution[2] = -1.0;
+               num_sols = 1;
+       }
 
-       // 4. The Bit Masking Process
-       vector<bool> vertexAssigned(m_vertexCount, false);
-       vector<bool> edgeAssigned(m_edgeCount, false);
-       for (unsigned int t = 0; (t < m_triangleCount); ++t) 
+       // Discard negative solutions
+       if ((num_sols >= 1) && (solution[0] < 0)) 
        {
-               for (unsigned char i = 0; i < 3; ++i) 
+               --num_sols;
+               solution[0] = solution[num_sols];
+       }
+       if ((num_sols >= 2) && (solution[1] < 0)) 
+       {
+               --num_sols;
+               solution[1] = solution[num_sols];
+       }
+       if ((num_sols == 3) && (solution[2] < 0)) 
+       {
+               --num_sols;
+       }
+
+       // Sort
+       if (num_sols == 2) 
+       {
+               if (solution[0] > solution[1]) 
                {
-                       if (!vertexAssigned[m_triangles[t].getVertex(i)]) 
-                       {
-                               vertexAssigned[m_triangles[t].getVertex(i)] = true;
-                               setTriangleMark(m_triangleMarks[t], 1 << (2 + i));
-                       }
-                       if (!edgeAssigned[m_triangles[t].getEdge(i)]) 
-                       {
-                               edgeAssigned[m_triangles[t].getEdge(i)] = true;
-                               setTriangleMark(m_triangleMarks[t], 1 << (5 + i));
-                       }
+                       double tmp = solution[0];
+                       solution[0] = solution[1];
+                       solution[1] = tmp;
+               }
+       }
+       else if (num_sols == 3) 
+       {
+
+               // Bubblesort
+               if (solution[0] > solution[1]) {
+                       double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
+               }
+               if (solution[1] > solution[2]) {
+                       double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
+               }
+               if (solution[0] > solution[1]) {
+                       double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
                }
        }
-       */
+
+       return num_sols;
 }
 
 // w3 is not perfect
-void bvh_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3], double *w1, double *w2, double *w3)
+void collisions_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3)
 {
        double  tempV1[3], tempV2[3], tempV4[3];
        double  a,b,c,d,e,f;
@@ -205,19 +353,19 @@ void bvh_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3]
        d = (a * c - b * b);
        
        if (ABS(d) < ALMOST_ZERO) {
-               *w1 = *w2 = *w3 = 1.0f / 3.0f;
+               *w1 = *w2 = *w3 = 1.0 / 3.0;
                return;
        }
        
-       w1[0] = (e * c - b * f) / d;
+       w1[0] = (float)((e * c - b * f) / d);
        
        if(w1[0] < 0)
-               w1[0] = 0.0;
+               w1[0] = 0;
        
-       w2[0] = (f - b * w1[0]) / c;
+       w2[0] = (float)((f - b * (double)w1[0]) / c);
        
        if(w2[0] < 0)
-               w2[0] = 0.0;
+               w2[0] = 0;
        
        w3[0] = 1.0f - w1[0] - w2[0];
 }
@@ -230,528 +378,3 @@ DO_INLINE void interpolateOnTriangle(float to[3], float v1[3], float v2[3], floa
        VECADDMUL(to, v3, w3);
 }
 
-
-DO_INLINE void calculateFrictionImpulse(float to[3], float vrel[3], float normal[3], double normalVelocity,
-       double frictionConstant, double delta_V_n) 
-{
-       float vrel_t_pre[3];
-       float vrel_t[3];
-       VECSUBS(vrel_t_pre, vrel, normal, normalVelocity);
-       VECCOPY(to, vrel_t_pre);
-       VecMulf(to, MAX2(1.0f - frictionConstant * delta_V_n / INPR(vrel_t_pre,vrel_t_pre), 0.0f));
-}
-
-               
-int collision_static(ClothModifierData *clmd, ClothModifierData *coll_clmd, LinkNode **collision_list)
-{
-       unsigned int i = 0, numfaces = 0;
-       int result = 0;
-       LinkNode *search = NULL;
-       CollPair *collpair = NULL;
-       Cloth *cloth1, *cloth2;
-       MFace *face1, *face2;
-       double w1, w2, w3, u1, u2, u3, a1, a2, a3;
-       float v1[3], v2[3], relativeVelocity[3];
-       float magrelVel;
-       
-       cloth1 = clmd->clothObject;
-       cloth2 = coll_clmd->clothObject;
-       
-       numfaces = clmd->clothObject->numfaces;
-               
-       for(i = 0; i < numfaces; i++)
-       {
-               search = collision_list[i];
-               
-               while(search)
-               {
-                       collpair = search->link;
-                       
-                       face1 = &(cloth1->mfaces[collpair->face1]);
-                       face2 = &(cloth2->mfaces[collpair->face2]);
-                       
-                       // compute barycentric coordinates for both collision points
-                       
-                       if(!collpair->quadA)
-                       {       
-                               bvh_compute_barycentric(collpair->p1,
-                                                       cloth1->verts[face1->v1].txold,
-                                                       cloth1->verts[face1->v2].txold,
-                                                       cloth1->verts[face1->v3].txold, 
-                                                               &w1, &w2, &w3);
-                       }
-                       else
-                               bvh_compute_barycentric(collpair->p1,
-                                                       cloth1->verts[face1->v4].txold,
-                                                       cloth1->verts[face1->v1].txold,
-                                                       cloth1->verts[face1->v3].txold, 
-                                                       &w1, &w2, &w3);
-                       
-                       if(!collpair->quadB)
-                               bvh_compute_barycentric(collpair->p2,
-                                                       cloth2->verts[face2->v1].txold,
-                                                       cloth2->verts[face2->v2].txold,
-                                                       cloth2->verts[face2->v3].txold, 
-                                                       &u1, &u2, &u3);
-                       else
-                               bvh_compute_barycentric(collpair->p2,
-                                                       cloth2->verts[face2->v4].txold,
-                                                       cloth2->verts[face2->v1].txold,
-                                                       cloth2->verts[face2->v3].txold, 
-                                                       &u1, &u2, &u3);
-                       
-                       // Calculate relative "velocity".
-                       
-                       if(!collpair->quadA)
-                               interpolateOnTriangle(v1, cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv, cloth1->verts[face1->v3].tv, w1, w2, w3);
-                       else
-                               interpolateOnTriangle(v1, cloth1->verts[face1->v4].tv, cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv, w1, w2, w3);
-                       
-                       if(!collpair->quadB)
-                               interpolateOnTriangle(v2, cloth2->verts[face2->v1].tv, cloth2->verts[face2->v2].tv, cloth2->verts[face2->v3].tv, u1, u2, u3);
-                       else
-                               interpolateOnTriangle(v2, cloth2->verts[face2->v4].tv, cloth2->verts[face2->v1].tv, cloth2->verts[face2->v3].tv, u1, u2, u3);
-                       
-                       VECSUB(relativeVelocity, v1, v2);
-                               
-                       // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
-                       magrelVel = INPR(relativeVelocity, collpair->normal);
-                                       
-                       // Calculate masses of points.
-                       
-                       // If v_n_mag > 0 the edges are approaching each other.
-                       
-                       if(magrelVel < -ALMOST_ZERO)
-                       {
-                               // Calculate Impulse magnitude to stop all motion in normal direction.
-                               // const double I_mag = v_n_mag / (1/m1 + 1/m2);
-                               float magnitude_i = magrelVel / 2.0f; // TODO implement masses
-                               float tangential[3], magtangent, magnormal, collvel[3];
-                               float vrel_t_pre[3];
-                               float vrel_t[3];
-                               double impulse;
-                               float epsilon = clmd->coll_parms.epsilon;
-                               float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
-                               
-                               // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms.friction*0.01, magrelVel);
-                               
-                               // magtangent = INPR(tangential, tangential);
-                               
-                               // Apply friction impulse.
-                               if (magtangent < ALMOST_ZERO) 
-                               {
-                                       
-                                       // printf("friction applied: %f\n", magtangent);
-                                       // TODO check original code 
-                                       /*
-                                       VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,tangential);
-                                       VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv,tangential);
-                                       VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv,tangential);
-                                       VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v4].tv,tangential);
-                                       */
-                               }
-                               
-                               impulse = -magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
-                               VECADDMUL(cloth1->verts[face1->v1].impulse, collpair->normal, impulse); 
-                               cloth1->verts[face1->v1].impulse_count++;
-                               
-                               VECADDMUL(cloth1->verts[face1->v2].impulse, collpair->normal, impulse); 
-                               cloth1->verts[face1->v2].impulse_count++;
-                               
-                               VECADDMUL(cloth1->verts[face1->v3].impulse, collpair->normal, impulse); 
-                               cloth1->verts[face1->v3].impulse_count++;
-                               
-                               if(face1->v4)
-                               {
-                                       VECADDMUL(cloth1->verts[face1->v4].impulse, collpair->normal, impulse); 
-                                       cloth1->verts[face1->v4].impulse_count++;
-                               }
-                               
-                               
-                               if (overlap > ALMOST_ZERO) {
-                                       double I_mag  = overlap * 0.1;
-                                       
-                                       impulse = I_mag / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
-                                       
-                                       VECADDMUL(cloth1->verts[face1->v1].impulse, collpair->normal, impulse); 
-                                       cloth1->verts[face1->v1].impulse_count++;
-                                                               
-                                       VECADDMUL(cloth1->verts[face1->v2].impulse, collpair->normal, impulse); 
-                                       cloth1->verts[face1->v2].impulse_count++;
-                               
-                                       VECADDMUL(cloth1->verts[face1->v3].impulse, collpair->normal, impulse); 
-                                       cloth1->verts[face1->v3].impulse_count++;
-                               
-                                       if(face1->v4)
-                                       {
-                                               VECADDMUL(cloth1->verts[face1->v4].impulse, collpair->normal, impulse); 
-                                               cloth1->verts[face1->v4].impulse_count++;
-                                       }
-                               
-                               }
-                               
-                               result = 1;
-                               
-                       
-                               // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
-                               
-                               // Apply the impulse and increase impulse counters.
-
-                               /*                      
-                               // calculateFrictionImpulse(tangential, collvel, collpair->normal, magtangent, clmd->coll_parms.friction*0.01, magtangent);
-                               VECSUBS(vrel_t_pre, collvel, collpair->normal, magnormal);
-                               // VecMulf(vrel_t_pre, clmd->coll_parms.friction*0.01f/INPR(vrel_t_pre,vrel_t_pre));
-                               magtangent = Normalize(vrel_t_pre);
-                               VecMulf(vrel_t_pre, MIN2(clmd->coll_parms.friction*0.01f*magnormal,magtangent));
-                               
-                               VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,vrel_t_pre);
-                               */
-                               
-                               
-                               
-                       }
-                       
-                       search = search->next;
-               }
-       }
-               
-       return result;
-}
-
-// return distance between two triangles using bullet engine
-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)
-{
-       MFace *face1=NULL, *face2=NULL;
-       float  a[3][3];
-       float  b[3][3];
-       double distance=0, tempdistance=0;
-       Cloth *cloth1=NULL, *cloth2=NULL;
-       float tpa[3], tpb[3], tnormal[3];
-       unsigned int indexA=0, indexB=0, indexC=0, indexD=0, indexE=0, indexF=0;
-       int i = 0;
-       
-       cloth1 = clmd->clothObject;
-       cloth2 = coll_clmd->clothObject;
-       
-       face1 = &(cloth1->mfaces[tri_index1]);
-       face2 = &(cloth2->mfaces[tri_index2]);
-       
-       // face a1 + face b1
-       VECCOPY(a[0], cloth1->verts[face1->v1].txold);
-       VECCOPY(a[1], cloth1->verts[face1->v2].txold);
-       VECCOPY(a[2], cloth1->verts[face1->v3].txold);
-       
-       
-       VECCOPY(b[0], cloth2->verts[face2->v1].txold);
-       VECCOPY(b[1], cloth2->verts[face2->v2].txold);
-       VECCOPY(b[2], cloth2->verts[face2->v3].txold);
-
-       distance = plNearestPoints(a,b,pa,pb,normal);
-       
-       quadA = quadB = 0;
-       
-       for(i = 0; i < 3; i++)
-       {
-               if(i == 0)
-               {
-                       if(face1->v4)
-                       {
-                               indexA = face1->v4;
-                               indexB = face1->v1;
-                               indexC = face1->v3;
-                               
-                               indexD = face2->v1;
-                               indexE = face2->v2;
-                               indexF = face2->v3;
-                       }
-                       else 
-                               i+=2;
-               }
-               
-               if(i == 1)
-               {
-                       if((face1->v4)&&(face2->v4))
-                       {
-                               indexA = face1->v4;
-                               indexB = face1->v1;
-                               indexC = face1->v3;
-                       
-                               indexD = face2->v4;
-                               indexE = face2->v1;
-                               indexF = face2->v3;
-                       }
-                       else
-                               i++;
-               }
-               
-               if(i == 2)
-               {
-                       if(face2->v4)
-                       {
-                               indexA = face1->v1;
-                               indexB = face1->v2;
-                               indexC = face1->v3;
-                       
-                               indexD = face2->v4;
-                               indexE = face2->v1;
-                               indexF = face2->v3;
-                       }
-                       else
-                               i++;
-                       
-               }
-               
-               if(i<3)
-               {
-                       // face a2 + face b1
-                       VECCOPY(a[0], cloth1->verts[indexA].txold);
-                       VECCOPY(a[1], cloth1->verts[indexB].txold);
-                       VECCOPY(a[2], cloth1->verts[indexC].txold);
-                       
-                       
-                       VECCOPY(b[0], cloth2->verts[indexD].txold);
-                       VECCOPY(b[1], cloth2->verts[indexE].txold);
-                       VECCOPY(b[2], cloth2->verts[indexF].txold);
-       
-                       tempdistance = plNearestPoints(a,b,tpa,tpb,tnormal);
-                       
-                       if(tempdistance < distance)
-                       {
-                               VECCOPY(pa, tpa);
-                               VECCOPY(pb, tpb);
-                               VECCOPY(normal, tnormal);
-                               distance = tempdistance;
-                               
-                               if(i == 0)
-                               {
-                                       quadA = 1; quadB = 0;
-                               }
-                               else if(i == 1)
-                               {
-                                       quadA = quadB = 1;
-                               }
-                               else if(i == 2)
-                               {
-                                       quadA = 0; quadB = 1;
-                               }
-                       }
-               }
-       }
-       return distance;
-}
-
-void bvh_collision_response(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree * tree1, Tree * tree2)
-{
-       CollPair *collpair = NULL;
-       LinkNode **linknode;
-       double distance = 0;
-       float epsilon = clmd->coll_parms.epsilon;
-
-       collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");
-       linknode = clmd->coll_parms.temp;
-       
-       // calc SIPcode (?)
-       
-       // calc distance + normal       
-       distance = implicit_tri_check_coherence(clmd, coll_clmd, tree1->tri_index, tree2->tri_index, collpair->p1, collpair->p2, collpair->vector, collpair->quadA, collpair->quadB);
-       
-       if ((distance <= (epsilon + ALMOST_ZERO)) && (distance > -1.0f)) // max overlap = 1.0 
-       {
-               // printf("dist: %f\n", (float)distance);
-               
-               collpair->face1 = tree1->tri_index;
-               collpair->face2 = tree2->tri_index;
-               
-               VECCOPY(collpair->normal, collpair->vector);
-               Normalize(collpair->normal);
-               
-               collpair->distance = distance;
-               BLI_linklist_append(&linknode[tree1->tri_index], collpair);
-       }
-       else
-       {
-               MEM_freeN(collpair);
-       }
-}
-
-// move collision objects forward in time and update static bounding boxes
-void cloth_update_collision_objects(float step)
-{
-       Base *base=NULL;
-       ClothModifierData *coll_clmd=NULL;
-       Object *coll_ob=NULL;
-       unsigned int i=0;
-       
-       // search all objects for collision object
-       for (base = G.scene->base.first; base; base = base->next)
-       {
-
-               coll_ob = base->object;
-               coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
-               if (!coll_clmd)
-                       continue;
-
-               // if collision object go on
-               if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
-               {
-                       if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
-                       {
-                               Cloth *coll_cloth = coll_clmd->clothObject;
-                               BVH *coll_bvh = coll_clmd->clothObject->tree;
-                               unsigned int coll_numverts = coll_cloth->numverts;
-
-                               // update position of collision object
-                               for(i = 0; i < coll_numverts; i++)
-                               {
-                                       VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
-
-                                       VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
-                                       
-                                       // no dt here because of float rounding errors
-                                       VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
-                               }
-                                               
-                               // update BVH of collision object
-                               bvh_update_static(coll_clmd, coll_bvh);
-                       }
-                       else
-                               printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
-               }
-       }
-}
-
-#define CLOTH_MAX_THRESHOLD 5
-
-// cloth - object collisions
-int cloth_bvh_objcollision(ClothModifierData * clmd, float step, CM_COLLISION_RESPONSE collision_response, float dt)
-{
-       Base *base=NULL;
-       ClothModifierData *coll_clmd=NULL;
-       Cloth *cloth=NULL;
-       Object *coll_ob=NULL;
-       BVH *cloth_bvh=NULL;
-       unsigned int i=0, numfaces = 0, numverts = 0;
-       unsigned int result = 0, ic = 0, rounds = 0;
-       ClothVertex *verts = NULL;
-       float tnull[3] = {0,0,0};
-
-       if ((clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
-       {
-               return 0;
-       }
-       cloth = clmd->clothObject;
-       verts = cloth->verts;
-       cloth_bvh = (BVH *) cloth->tree;
-       numfaces = clmd->clothObject->numfaces;
-       numverts = clmd->clothObject->numverts;
-       
-       ////////////////////////////////////////////////////////////
-       // static collisions
-       ////////////////////////////////////////////////////////////
-
-       // update cloth bvh
-       bvh_update_static(clmd, cloth_bvh);
-       
-       // update collision objects
-       cloth_update_collision_objects(step);
-
-       do
-       {
-               result = 0;
-               ic = 0;
-                       
-               // handle all collision objects
-               for (base = G.scene->base.first; base; base = base->next)
-               {
-       
-                       coll_ob = base->object;
-                       coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
-                       if (!coll_clmd)
-                               continue;
-       
-                       // if collision object go on
-                       if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
-                       {
-                               if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
-                               {
-                                       LinkNode **collision_list = MEM_callocN (sizeof(LinkNode *)*(numfaces), "collision_list");
-                                       BVH *coll_bvh = coll_clmd->clothObject->tree;
-       
-                                       if(collision_list)
-                                       {                                       
-                                               memset(collision_list, 0, sizeof(LinkNode *)*numfaces);
-                                               clmd->coll_parms.temp = collision_list;
-       
-                                               bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, collision_response);
-                                               
-                                               result += collision_static(clmd, coll_clmd, collision_list);
-                                               
-                                               // calculate velocities
-                                               
-                                               // free temporary list 
-                                               for(i = 0; i < numfaces; i++)
-                                               {
-                                                       LinkNode *search = collision_list[i];
-                                                       while(search)
-                                                       {
-                                                               LinkNode *next= search->next;
-                                                               CollPair *collpair = search->link;
-                                                               
-                                                               if(collpair)
-                                                                       MEM_freeN(collpair);    
-       
-                                                               search = next;
-                                                       }
-       
-                                                       BLI_linklist_free(collision_list[i],NULL); 
-                                               }
-                                               if(collision_list)
-                                                       MEM_freeN(collision_list);
-       
-                                               clmd->coll_parms.temp = NULL;
-                                       }
-                                       
-       
-                               }
-                               else
-                                       printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
-                       }
-               }
-               
-               // now apply impulses parallel
-               
-               for(i = 0; i < numverts; i++)
-               {
-                       if(verts[i].impulse_count)
-                       {
-                               VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
-                               VECCOPY(verts[i].impulse, tnull);
-                               verts[i].impulse_count = 0;
-                               
-                               ic++;
-                       }
-               }
-               
-               printf("ic: %d\n", ic);
-               rounds++;
-       }
-       while(result && (CLOTH_MAX_THRESHOLD>rounds));
-       
-       printf("\n");
-                       
-       ////////////////////////////////////////////////////////////
-       // update positions + velocities
-       ////////////////////////////////////////////////////////////
-
-       // TODO 
-
-
-       ////////////////////////////////////////////////////////////
-       // moving collisions
-       ////////////////////////////////////////////////////////////
-
-       // TODO 
-       // bvh_update_moving(clmd, clmd->clothObject->tree);
-
-       return MIN2(result, 1);
-}