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 7eec315e386de15af8dd9b7835bce887f25b179c..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
-
-
-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 
-};
+// 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);
+       }
+}
 
-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; }
 
+/**
+ * gsl_poly_solve_cubic -
+ *
+ * copied from SOLVE_CUBIC.C --> GSL
+ */
+#define mySWAP(a,b) { float tmp = b ; b = a ; a = tmp ; }
 
-void generateTriangleMarks() 
+int gsl_poly_solve_cubic (float a, float b, float c, float *x0, float *x1, float *x2)
 {
-       /*
-       unsigned int firstEdge = 0;
-       
-       // 1. Initialization
-       memset(m_triangleMarks, 0, sizeof(unsigned char) * m_triangleCount);
+       float q = (a * a - 3 * b);
+       float r = (2 * a * a * a - 9 * a * b + 27 * c);
 
-       // 2. The Marking Process
-       
-       // 2.1 Randomly mark triangles for covering vertices.
-       for (unsigned int v = 0; v < m_vertexCount; ++v) 
-       {
-               if (vertexCover(v) == 0) 
-               {
+       float Q = q / 9;
+       float R = r / 54;
 
-                       // Randomly select an edge whose first triangle we're going to flag. 
+       float Q3 = Q * Q * Q;
+       float R2 = R * R;
 
-#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);
-                       }
-               }
-       }
-       */
-       /* 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) 
+       float CR2 = 729 * r * r;
+       float CQ3 = 2916 * q * q * q;
+
+       if (R == 0 && Q == 0)
        {
-               if (m_edges[e].getTriangleCount() && (edgeCover(e) == 0)) 
-               {
-#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
-               }
+               *x0 = - a / 3 ;
+               *x1 = - a / 3 ;
+               *x2 = - a / 3 ;
+               return 3 ;
        }
-
-       
-       // 3. The Unmarking Process
-       for (unsigned int t = 0; (t < m_triangleCount); ++t) 
+       else if (CR2 == CQ3) 
        {
-               bool overCoveredVertices = true;
-               bool overCoveredEdges = true;
-               for (unsigned char i = 0; (i < 3) && (overCoveredVertices || overCoveredEdges); ++i) 
-               {
+         /* this test is actually R2 == Q3, written in a form suitable
+               for exact computation with integers */
+
+         /* 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. */
 
-                       if (vertexCover(m_triangles[t].getVertex(i)) == 1)
-                               overCoveredVertices = false;
-                       if (edgeCover(m_triangles[t].getEdge(i)) == 1)
-                               overCoveredEdges = false;
+               float sqrtQ = sqrtf (Q);
 
-                       assert(vertexCover(m_triangles[t].getVertex(i)) > 0);
-                       assert(edgeCover(m_triangles[t].getEdge(i)) > 0);
+               if (R > 0)
+               {
+                       *x0 = -2 * sqrtQ  - a / 3;
+                       *x1 = sqrtQ - a / 3;
+                       *x2 = sqrtQ - a / 3;
                }
-               if (overCoveredVertices)
-                       clearTriangleMark(m_triangleMarks[t], TM_MV);
-               if (overCoveredEdges)
-                       clearTriangleMark(m_triangleMarks[t], TM_ME);
+               else
+               {
+                       *x0 = - sqrtQ  - a / 3;
+                       *x1 = - sqrtQ - a / 3;
+                       *x2 = 2 * sqrtQ - a / 3;
+               }
+               return 3 ;
        }
-
-
-       // 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) 
+       else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
        {
-               for (unsigned char i = 0; i < 3; ++i) 
+               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)
                {
-                       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));
-                       }
+                       mySWAP(*x1, *x2) ;
+          
+                       if (*x0 > *x1)
+                               mySWAP(*x0, *x1) ;
                }
+      
+               return 3;
        }
-       */
-}
-
-// 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)
-{
-       double  tempV1[3], tempV2[3], tempV4[3];
-       double  a,b,c,d,e,f;
-
-       VECSUB (tempV1, p1, p3);        
-       VECSUB (tempV2, p2, p3);        
-       VECSUB (tempV4, pv, p3);        
-       
-       a = INPR (tempV1, tempV1);      
-       b = INPR (tempV1, tempV2);      
-       c = INPR (tempV2, tempV2);      
-       e = INPR (tempV1, tempV4);      
-       f = INPR (tempV2, tempV4);      
-       
-       d = (a * c - b * b);
-       
-       if (ABS(d) < ALMOST_ZERO) {
-               *w1 = *w2 = *w3 = 1.0f / 3.0f;
-               return;
+       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;
        }
-       
-       w1[0] = (e * c - b * f) / d;
-       
-       w2[0] = (f - b * w1[0]) / c;
-       
-       w3[0] = 1.0f - w1[0] - w2[0];
-}
-
-DO_INLINE void interpolateOnTriangle(float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3) 
-{
-       to[0] = to[1] = to[2] = 0;
-       VECADDMUL(to, v1, w1);
-       VECADDMUL(to, v2, w2);
-       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) 
+/**
+ * gsl_poly_solve_quadratic
+ *
+ * copied from GSL
+ */
+int gsl_poly_solve_quadratic (float a, float b, float c,  float *x0, float *x1)
 {
-       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));
-}
+       float disc = b * b - 4 * a * c;
 
-/*
-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++)
+       if (disc > 0)
        {
-               search = collision_list[i];
-               
-               while(search)
+               if (b == 0)
                {
-                       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;
+                       float r = fabs (0.5 * sqrtf (disc) / a);
+                       *x0 = -r;
+                       *x1 =  r;
                }
-       }
-               
-       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)
+               else
                {
-                       if(face1->v4)
+                       float sgnb = (b > 0 ? 1 : -1);
+                       float temp = -0.5 * (b + sgnb * sqrtf (disc));
+                       float r1 = temp / a ;
+                       float r2 = c / temp ;
+
+                       if (r1 < r2) 
                        {
-                               indexA = face1->v4;
-                               indexB = face1->v1;
-                               indexC = face1->v3;
-                               
-                               indexD = face2->v1;
-                               indexE = face2->v2;
-                               indexF = face2->v3;
-                       }
+                               *x0 = r1 ;
+                               *x1 = r2 ;
+                       } 
                        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;
-                               }
+                               *x0 = r2 ;
+                               *x1 = r1 ;
                        }
                }
+               return 2;
+       }
+       else if (disc == 0) 
+       {
+               *x0 = -0.5 * b / a ;
+               *x1 = -0.5 * b / a ;
+               return 2 ;
+       }
+       else
+       {
+               return 0;
        }
-       return distance;
 }
 
-// calculate plane normal
-void calcPlaneNormal(float normal[3], float p11[3], float p12[3], float p13[3])
-{
-       float temp1[3], temp2[3];
-       float tnormal[3];
-       
-       VECSUB(temp1, p12,p11);
-       VECSUB(temp2, p13,p11);
-       Crossf(normal, temp1, temp2);
-       Normalize(normal);
-       // VECCOPY(normal, tnormal);
-}
 
-float distance_triangle_point( float p11[3], float p12[3], float p13[3], float p21[3], float normal[3])
+
+/*
+ * 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]) 
 {
-       float temp[3];
-       float magnitude = 0;
-       
-       VECSUB(temp, p21, p13);
-       magnitude = INPR(temp, normal);
-       
-       if(magnitude < 0)
+       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)
        {
-               magnitude *= -1.0f;
-               // VecMulf(normal, -1.0f);
+               i /= j;
+               h /= j;
+               g /= j;
+               
+               num_sols = gsl_poly_solve_cubic(i, h, g, &solution[0], &solution[1], &solution[2]);
        }
-       
-       return magnitude;
-}
-
-float nearest_point_triangle_triangle(float p11[3], float p12[3], float p13[3], float p21[3], float p22[3], float p23[3], float normal[3])
-{
-       float distance = 0, tdistance = 0, tnormal[3];
-       
-       // first triangle 1-2-3 versus second triangle 1-2-3
-       calcPlaneNormal(normal, p11, p12, p13);
-       distance = distance_triangle_point(p11, p12, p13, p21, normal);
-       
-       tdistance = distance_triangle_point(p11, p12, p13, p22, normal);
-       
-       if(tdistance < distance)
+       else if(ABS(i) > ALMOST_ZERO)
        {       
-               distance = tdistance;
+               num_sols = gsl_poly_solve_quadratic(i, h, g, &solution[0], &solution[1]);
+               solution[2] = -1.0;
        }
-       
-       tdistance = distance_triangle_point(p11, p12, p13, p23, normal);
-       
-       if(tdistance < distance)
-       {       
-               distance = tdistance;
+       else if(ABS(h) > ALMOST_ZERO)
+       {
+               solution[0] = -g / h;
+               solution[1] = solution[2] = -1.0;
+               num_sols = 1;
        }
-       
-       // second triangle 1-2-3 versus first triangle 1-2-3
-       calcPlaneNormal(tnormal, p21, p22, p23);
-       
-       tdistance = distance_triangle_point(p21, p22, p23, p11, tnormal);
-       
-       if(tdistance < distance)
-       {       
-               distance = tdistance;
-               VECCOPY(normal, tnormal);
+       else if(ABS(g) > ALMOST_ZERO)
+       {
+               solution[0] = 0;
+               solution[1] = solution[2] = -1.0;
+               num_sols = 1;
        }
-       
-       tdistance = distance_triangle_point(p21, p22, p23, p12, tnormal);
-       
-       if(tdistance < distance)
-       {       
-               distance = tdistance;
-               VECCOPY(normal, tnormal);
+
+       // Discard negative solutions
+       if ((num_sols >= 1) && (solution[0] < 0)) 
+       {
+               --num_sols;
+               solution[0] = solution[num_sols];
        }
-       
-       tdistance = distance_triangle_point(p21, p22, p23, p13, tnormal);
-       
-       if(tdistance < distance)
-       {       
-               distance = tdistance;
-               VECCOPY(normal, tnormal);
+       if ((num_sols >= 2) && (solution[1] < 0)) 
+       {
+               --num_sols;
+               solution[1] = solution[num_sols];
        }
-       
-       
-       if (distance < 0) {
-               VecMulf(normal, -1.0f);
-               distance = -distance;
+       if ((num_sols == 3) && (solution[2] < 0)) 
+       {
+               --num_sols;
        }
-       
-       return distance;        
-}
 
-               
-int collision_static2(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++)
+       // Sort
+       if (num_sols == 2) 
        {
-               search = collision_list[i];
-               
-               while(search)
+               if (solution[0] > solution[1]) 
                {
-                       collpair = search->link;
-                       
-                       face1 = &(cloth1->mfaces[collpair->face1]);
-                       face2 = &(cloth2->mfaces[collpair->face2]);
-                       
-                       // compute barycentric coordinates for both collision points
-                       
-               
-                       bvh_compute_barycentric(collpair->p1,
-                                       cloth1->verts[collpair->Aindex1].txold,
-                                       cloth1->verts[collpair->Aindex2].txold,
-                                       cloth1->verts[collpair->Aindex3].txold, 
-                                       &w1, &w2, &w3);
-               
-                       bvh_compute_barycentric(collpair->p2,
-                                       cloth2->verts[collpair->Bindex1].txold,
-                                       cloth2->verts[collpair->Bindex1].txold,
-                                       cloth2->verts[collpair->Bindex3].txold, 
-                                       &u1, &u2, &u3);
-                       
-                       // Calculate relative "velocity".
-                       interpolateOnTriangle(v1, cloth1->verts[collpair->Aindex1].tv, cloth1->verts[collpair->Aindex2].tv, cloth1->verts[collpair->Aindex3].tv, w1, w2, w3);
-               
-                       interpolateOnTriangle(v2, cloth2->verts[collpair->Bindex1].tv, cloth2->verts[collpair->Bindex2].tv, cloth2->verts[collpair->Bindex3].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);
-                               
-                               /*
-                               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 (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;
-                       }
-                       
-                       search = search->next;
+                       double tmp = solution[0];
+                       solution[0] = solution[1];
+                       solution[1] = tmp;
                }
        }
-               
-       return result;
-}
-
-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, tdistance=0;
-       MFace *face1, *face2;
-       ClothVertex *verts1, *verts2;
-       Cloth *cloth1=NULL, *cloth2=NULL;
-       int i = 0;
-       
-       linknode = clmd->coll_parms.temp;
-       
-       cloth1 = clmd->clothObject;
-       cloth2 = coll_clmd->clothObject;
-       
-       // calc SIPcode (?)
-       
-       for(i = 0; i < 4; i++)
+       else if (num_sols == 3) 
        {
-               collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");
-               
-               face1 = &(cloth1->mfaces[tree1->tri_index]);
-               face2 = &(cloth2->mfaces[tree2->tri_index]);
-                       
-               verts1 = cloth1->verts;
-               verts2 = cloth2->verts;
-               
-               if(i == 0)
-               {
-                       collpair->Aindex1 = face1->v1;
-                       collpair->Aindex2 = face1->v2;
-                       collpair->Aindex3 = face1->v3;
-                       collpair->Aindex4 = face1->v4;
-                       
-                       collpair->Bindex1 = face2->v1;
-                       collpair->Bindex2 = face2->v2;
-                       collpair->Bindex3 = face2->v3;
-                       collpair->Bindex4 = face2->v4;
-                       
-               }
-               
-               if(i == 1)
-               {
-                       if(face2->v4)
-                       {               
-                               collpair->Aindex1 = face1->v1;
-                               collpair->Aindex2 = face1->v2;
-                               collpair->Aindex3 = face1->v3;
-                               collpair->Aindex4 = face1->v4;
-                               
-                               collpair->Bindex1 = face2->v4;
-                               collpair->Bindex2 = face2->v3;
-                               collpair->Bindex3 = face2->v1;
-                               collpair->Bindex4 = face2->v1;
-                       }
-                       else
-                               i++;
-                       
+
+               // Bubblesort
+               if (solution[0] > solution[1]) {
+                       double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
                }
-               
-               if(i == 2)
-               {
-                       if(face1->v4)
-                       {               
-                               collpair->Aindex1 = face1->v4;
-                               collpair->Aindex2 = face1->v3;
-                               collpair->Aindex3 = face1->v1;
-                               collpair->Aindex4 = face1->v2;
-                               
-                               collpair->Bindex1 = face2->v1;
-                               collpair->Bindex2 = face2->v2;
-                               collpair->Bindex3 = face2->v3;
-                               collpair->Bindex4 = face2->v4;
-                       }
-                       else
-                               i++;
+               if (solution[1] > solution[2]) {
+                       double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
                }
-               
-               if(i == 3)
-               {
-                       if((face2->v4) && (face1->v4))
-                       {               
-                               collpair->Aindex1 = face1->v4;
-                               collpair->Aindex2 = face1->v3;
-                               collpair->Aindex3 = face1->v1;
-                               collpair->Aindex4 = face1->v2;
-                               
-                               collpair->Bindex1 = face2->v4;
-                               collpair->Bindex2 = face2->v3;
-                               collpair->Bindex3 = face2->v1;
-                               collpair->Bindex4 = face2->v2;
-                       }
-                       else
-                               i++;
-               }
-               
-               if(i < 4)
-               {
-                       distance = nearest_point_triangle_triangle(verts1[collpair->Aindex1].txold, verts1[collpair->Aindex2].txold, verts1[collpair->Aindex3].txold, verts2[collpair->Bindex1].txold, verts2[collpair->Bindex2].txold, verts2[collpair->Bindex3].txold, collpair->normal);
-                       
-                       // 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)) // max overlap = 1.0 
-                       {
-                               
-                               printf("dist: %f, tdist: %f\n", (float)distance, tdistance);
-                               
-                               collpair->face1 = tree1->tri_index;
-                               collpair->face2 = tree2->tri_index;
-                               
-                               collpair->distance = distance;
-                               BLI_linklist_append(&linknode[tree1->tri_index], collpair);
-                       }
-                       else
-                       {
-                               MEM_freeN(collpair);
-                       }
+               if (solution[0] > solution[1]) {
+                       double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
                }
        }
-}
-
-// 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");
-               }
-       }
+       return num_sols;
 }
 
-#define CLOTH_MAX_THRESHOLD 5
-
-// cloth - object collisions
-int cloth_bvh_objcollision(ClothModifierData * clmd, float step, CM_COLLISION_RESPONSE collision_response, float dt)
+// w3 is not perfect
+void collisions_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3)
 {
-       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);
+       double  tempV1[3], tempV2[3], tempV4[3];
+       double  a,b,c,d,e,f;
 
-       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;
+       VECSUB (tempV1, p1, p3);        
+       VECSUB (tempV2, p2, p3);        
+       VECSUB (tempV4, pv, p3);        
        
-                       // 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;
+       a = INPR (tempV1, tempV1);      
+       b = INPR (tempV1, tempV2);      
+       c = INPR (tempV2, tempV2);      
+       e = INPR (tempV1, tempV4);      
+       f = INPR (tempV2, tempV4);      
        
-                                       if(collision_list)
-                                       {                                       
-                                               memset(collision_list, 0, sizeof(LinkNode *)*numfaces);
-                                               clmd->coll_parms.temp = collision_list;
+       d = (a * c - b * b);
        
-                                               bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, collision_response);
-                                               
-                                               result += collision_static2(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);    
+       if (ABS(d) < ALMOST_ZERO) {
+               *w1 = *w2 = *w3 = 1.0 / 3.0;
+               return;
+       }
        
-                                                               search = next;
-                                                       }
+       w1[0] = (float)((e * c - b * f) / d);
        
-                                                       BLI_linklist_free(collision_list[i],NULL); 
-                                               }
-                                               if(collision_list)
-                                                       MEM_freeN(collision_list);
+       if(w1[0] < 0)
+               w1[0] = 0;
        
-                                               clmd->coll_parms.temp = NULL;
-                                       }
-                                       
+       w2[0] = (float)((f - b * (double)w1[0]) / c);
        
-                               }
-                               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));
+       if(w2[0] < 0)
+               w2[0] = 0;
        
-       printf("\n");
-                       
-       ////////////////////////////////////////////////////////////
-       // update positions + velocities
-       ////////////////////////////////////////////////////////////
-
-       // TODO 
-
-
-       ////////////////////////////////////////////////////////////
-       // moving collisions
-       ////////////////////////////////////////////////////////////
-
-       // TODO 
-       // bvh_update_moving(clmd, clmd->clothObject->tree);
+       w3[0] = 1.0f - w1[0] - w2[0];
+}
 
-       return MIN2(result, 1);
+DO_INLINE void interpolateOnTriangle(float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3) 
+{
+       to[0] = to[1] = to[2] = 0;
+       VECADDMUL(to, v1, w1);
+       VECADDMUL(to, v2, w2);
+       VECADDMUL(to, v3, w3);
 }
+