use static functions where possible for some local functions.
[blender.git] / source / blender / blenkernel / intern / collision.c
index 37784b43cf9bdd11f79718b0fa3469286c5f2f5a..8ef1c28537080f9fb3e20fc88872949227fee7af 100644 (file)
 
 #include "BKE_cloth.h"
 
-#include "DNA_group_types.h"
-#include "DNA_object_types.h"
 #include "DNA_cloth_types.h"
+#include "DNA_group_types.h"
 #include "DNA_mesh_types.h"
+#include "DNA_object_types.h"
+#include "DNA_object_force.h"
 #include "DNA_scene_types.h"
 
 #include "BKE_DerivedMesh.h"
 #include "BKE_modifier.h"
 #include "BKE_utildefines.h"
 #include "BKE_DerivedMesh.h"
-#include "mydevice.h"
 
 #include "Bullet-C-Api.h"
 
 #include "BLI_kdopbvh.h"
 #include "BKE_collision.h"
 
-#ifdef _WIN32
-static void start ( void )
-{}
-static void end ( void )
-{
-}
-static double val()
-{
-       return 0;
-}
-#else
-#include <sys/time.h>
-static void mystart ( struct timeval *start, struct timezone *z )
-{
-       gettimeofday ( start, z );
-}
-static void myend ( struct timeval *end, struct timezone *z )
-{
-       gettimeofday ( end,z );
-}
-static double myval ( struct timeval *start, struct timeval *end )
-{
-       double t1, t2;
-       t1 = ( double ) start->tv_sec + ( double ) start->tv_usec/ ( 1000*1000 );
-       t2 = ( double ) end->tv_sec + ( double ) end->tv_usec/ ( 1000*1000 );
-       return t2-t1;
-}
-#endif
 
 /***********************************
 Collision modifier code start
@@ -341,7 +313,7 @@ gsl_poly_solve_quadratic (double a, double b, double c,
 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
 *     page 4, left column
 */
-int cloth_get_collision_time ( double a[3], double b[3], double c[3], double d[3], double e[3], double f[3], double solution[3] )
+static int cloth_get_collision_time ( double a[3], double b[3], double c[3], double d[3], double e[3], double f[3], double solution[3] )
 {
        int num_sols = 0;
 
@@ -388,8 +360,8 @@ int cloth_get_collision_time ( double a[3], double b[3], double c[3], double d[3
        printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
 
        printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
-       */
-
+       
+*/
        // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
        if ( ABS ( j ) > DBL_EPSILON )
        {
@@ -453,8 +425,9 @@ int cloth_get_collision_time ( double a[3], double b[3], double c[3], double d[3
        return num_sols;
 }
 
+
 // w3 is not perfect
-void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
+static void collision_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;
@@ -568,7 +541,7 @@ int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifier
                        {
                                Normalize ( vrel_t_pre );
 
-                               impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
+                               impulse = magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // 2.0 * 
                                VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
                                VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
                                VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
@@ -618,7 +591,9 @@ CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap
        ClothModifierData *clmd = ( ClothModifierData * ) md1;
        CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
        MFace *face1=NULL, *face2 = NULL;
+#ifdef USE_BULLET
        ClothVertex *verts1 = clmd->clothObject->verts;
+#endif
        double distance = 0;
        float epsilon1 = clmd->coll_parms->epsilon;
        float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
@@ -694,7 +669,7 @@ CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap
                                break;
                }
 
-#ifdef WITH_BULLET
+#ifdef USE_BULLET
                // calc distance + normal
                distance = plNearestPoints (
                        verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, collmd->current_x[collpair->bp1].co, collmd->current_x[collpair->bp2].co, collmd->current_x[collpair->bp3].co, collpair->pa,collpair->pb,collpair->vector );
@@ -710,63 +685,54 @@ CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap
 
                        collpair->distance = distance;
                        collpair->flag = 0;
-               }
+                       collpair++;
+               }/*
                else
                {
-                       // check for collision in the future
-                       collpair->flag |= COLLISION_IN_FUTURE;
-               }
-               collpair++;
-       }
-       return collpair;
-}
-
-int cloth_are_edges_adjacent ( ClothModifierData *clmd, CollisionModifierData *collmd, EdgeCollPair *edgecollpair )
-{
-       Cloth *cloth1 = NULL;
-       ClothVertex *verts1 = NULL;
-       float temp[3];
-       MVert *verts2 = collmd->current_x; // old x
-
-       cloth1 = clmd->clothObject;
-       verts1 = cloth1->verts;
-
-       VECSUB ( temp, verts1[edgecollpair->p11].txold, verts2[edgecollpair->p21].co );
-       if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
-               return 1;
-
-       VECSUB ( temp, verts1[edgecollpair->p11].txold, verts2[edgecollpair->p22].co );
-       if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
-               return 1;
+                       float w1, w2, w3, u1, u2, u3;
+                       float v1[3], v2[3], relativeVelocity[3];
 
-       VECSUB ( temp, verts1[edgecollpair->p12].txold, verts2[edgecollpair->p21].co );
-       if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
-               return 1;
+                       // calc relative velocity
+                       
+                       // compute barycentric coordinates for both collision points
+                       collision_compute_barycentric ( collpair->pa,
+                       verts1[collpair->ap1].txold,
+                       verts1[collpair->ap2].txold,
+                       verts1[collpair->ap3].txold,
+                       &w1, &w2, &w3 );
 
-       VECSUB ( temp, verts1[edgecollpair->p12].txold, verts2[edgecollpair->p22].co );
-       if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
-               return 1;
+                       // was: txold
+                       collision_compute_barycentric ( collpair->pb,
+                       collmd->current_x[collpair->bp1].co,
+                       collmd->current_x[collpair->bp2].co,
+                       collmd->current_x[collpair->bp3].co,
+                       &u1, &u2, &u3 );
 
-       VECSUB ( temp, verts1[edgecollpair->p11].txold, verts1[edgecollpair->p12].txold );
-       if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
-               return 1;
+                       // Calculate relative "velocity".
+                       collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
 
-       VECSUB ( temp, verts2[edgecollpair->p21].co, verts2[edgecollpair->p22].co );
-       if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
-               return 1;
+                       collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
 
+                       VECSUB ( relativeVelocity, v2, v1 );
 
-       return 0;
+                       if(sqrt(INPR(relativeVelocity, relativeVelocity)) >= distance)
+                       {
+                               // check for collision in the future
+                               collpair->flag |= COLLISION_IN_FUTURE;
+                               collpair++;
+                       }
+               }*/
+       }
+       return collpair;
 }
 
-int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
+static int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
 {
        int result = 0;
        Cloth *cloth1;
        float w1, w2, w3, u1, u2, u3;
        float v1[3], v2[3], relativeVelocity[3];
        float magrelVel;
-       float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
 
        cloth1 = clmd->clothObject;
 
@@ -805,7 +771,7 @@ int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierD
                if ( magrelVel > ALMOST_ZERO )
                {
                        // Calculate Impulse magnitude to stop all motion in normal direction.
-                       float magtangent = 0, repulse = 0, d = 0;
+                       float magtangent = 0;
                        double impulse = 0.0;
                        float vrel_t_pre[3];
                        float temp[3];
@@ -879,13 +845,18 @@ static float projectPointOntoLine(float *p, float *a, float *b)
 
 static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal) 
 {
-       float line1[33], line2[3];
+       float line1[3], line2[3];
        float length;
 
        VECSUB(line1, np2, np1);
        VECSUB(line2, np3, np1);
 
+       // printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]);
+
        Crossf(out_normal, line1, line2);
+
+       
+
        length = Normalize(out_normal);
        if (length <= FLT_EPSILON)
        { // lines are collinear
@@ -920,7 +891,7 @@ static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, floa
 }
 
 // calculates the distance of 2 edges
-float edgedge_distance(float np11[3], float np12[3], float np21[3], float np22[3], float *out_a1, float *out_a2, float *out_normal)
+static float edgedge_distance(float np11[3], float np12[3], float np21[3], float np22[3], float *out_a1, float *out_a2, float *out_normal)
 {
        float line1[3], line2[3], cross[3];
        float length;
@@ -1094,19 +1065,18 @@ float edgedge_distance(float np11[3], float np12[3], float np21[3], float np22[3
        return 0;
 }
 
-int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
+static int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
 {
        EdgeCollPair edgecollpair;
        Cloth *cloth1=NULL;
        ClothVertex *verts1=NULL;
-       unsigned int i = 0, j = 0, k = 0;
+       unsigned int i = 0, k = 0;
        int numsolutions = 0;
        double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
-       double solution[3];
+       double solution[3], solution2[3];
        MVert *verts2 = collmd->current_x; // old x
        MVert *velocity2 = collmd->current_v; // velocity
-       float mintime = FLT_MAX;
-       float distance;
+       float distance = 0;
        float triA[3][3], triB[3][3];
        int result = 0;
 
@@ -1189,17 +1159,15 @@ int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDat
                        edgecollpair.p21 = collpair->bp1;
                        edgecollpair.p22 = collpair->bp3;
                }
-
-               if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 6))
+               /*
+               if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16))
                        printf("Ahier!\n");
-               if((edgecollpair.p11 == 6) && (edgecollpair.p12 == 3))
+               if((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3))
                        printf("Ahier!\n");
+               */
 
-               if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
+               // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
                {
-                       // printf("Collision between:\n");
-                       // printf("p11: %d, p12: %d, p21: %d, p22: %d\n", edgecollpair.p11, edgecollpair.p12, edgecollpair.p21, edgecollpair.p22);
-
                        // always put coll points in p21/p22
                        VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
                        VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
@@ -1212,6 +1180,16 @@ int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDat
 
                        numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
 
+                       if((edgecollpair.p11 == 3 && edgecollpair.p12==16)|| (edgecollpair.p11==16 && edgecollpair.p12==3))
+                       {
+                               if(edgecollpair.p21==6 || edgecollpair.p22 == 6)
+                               {
+                                       printf("dist: %f, sol[k]: %lf, sol2[k]: %lf\n", distance, solution[k], solution2[k]);
+                                       printf("a1: %f, a2: %f, b1: %f, b2: %f\n", x1[0], x2[0], x3[0], v1[0]);
+                                       printf("b21: %d, b22: %d\n", edgecollpair.p21, edgecollpair.p22);
+                               }
+                       }
+
                        for ( k = 0; k < numsolutions; k++ )
                        {
                                // printf("sol %d: %lf\n", k, solution[k]);
@@ -1222,14 +1200,13 @@ int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDat
                                        float distance;
                                        float impulse = 0;
                                        float I_mag;
-                                       float m1, m2;
 
                                        // move verts
-                                       VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, mintime);
-                                       VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, mintime);
+                                       VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, solution[k]);
+                                       VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, solution[k]);
 
-                                       VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, mintime);
-                                       VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, mintime);
+                                       VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, solution[k]);
+                                       VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, solution[k]);
 
                                        // TODO: check for collisions
                                        distance = edgedge_distance(triA[0], triA[1], triB[0], triB[1], &a, &b, out_normal);
@@ -1255,13 +1232,14 @@ int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDat
                                                VECSUB(vrel_1_to_2, vrel_1_to_2, temp);
 
                                                out_normalVelocity = INPR(vrel_1_to_2, out_normal);
-
+/*
+                                               // this correction results in wrong normals sometimes?
                                                if(out_normalVelocity < 0.0)
                                                {
                                                        out_normalVelocity*= -1.0;
-                                                       VecMulf(out_normal, -1.0);
+                                                       VecNegf(out_normal);
                                                }
-
+*/
                                                /* Inelastic repulsion impulse. */
 
                                                // Calculate which normal velocity we need. 
@@ -1287,7 +1265,7 @@ int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDat
                                        {
                                                // missing from collision.hpp
                                        }
-                                       mintime = MIN2(mintime, (float)solution[k]);
+                                       // mintime = MIN2(mintime, (float)solution[k]);
 
                                        break;
                                }
@@ -1297,51 +1275,9 @@ int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierDat
        return result;
 }
 
-int cloth_collision_moving_tris ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
-{
-       EdgeCollPair edgecollpair;
-       Cloth *cloth1=NULL;
-       ClothVertex *verts1=NULL;
-       unsigned int i = 0, j = 0, k = 0;
-       int numsolutions = 0;
-       double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
-       double solution[3];
-       MVert *verts2 = collmd->current_x; // old x
-       MVert *velocity2 = collmd->current_v; // velocity
-       float mintime = FLT_MAX;
-       float distance;
-       float triA[3][3], triB[3][3];
-       int result = 0;
-
-       cloth1 = clmd->clothObject;
-       verts1 = cloth1->verts;
-
-       for(i = 0; i < 9; i++)
-       {
-               // 9 edge - edge possibilities
-
-               if(i == 0) 
-               {
-                       edgecollpair.p11 = collpair->ap1;
-                       edgecollpair.p12 = collpair->ap2;
-
-                       edgecollpair.p21 = collpair->bp1;
-                       edgecollpair.p22 = collpair->bp2;
-               }
-       }
-
-       return result;
-}
-
-int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
+static int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
 {
-       int result = 0;
        Cloth *cloth1;
-       float w1, w2, w3, u1, u2, u3;
-       float v1[3], v2[3], relativeVelocity[3];
-       float magrelVel;
-       float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
-
        cloth1 = clmd->clothObject;
 
        for ( ; collpair != collision_end; collpair++ )
@@ -1357,52 +1293,132 @@ int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *col
        return 1;
 }
 
-int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData *collmd, float step, float dt )
-{
-       Cloth *cloth = clmd->clothObject;
-       BVHTree *cloth_bvh= ( BVHTree * ) cloth->bvhtree;
-       long i=0, j = 0, numfaces = 0, numverts = 0;
-       ClothVertex *verts = NULL;
-       CollPair *collisions = NULL, *collisions_index = NULL;
-       int ret = 0;
-       int result = 0;
-       float tnull[3] = {0,0,0};
-       BVHTreeOverlap *overlap = NULL;
-
-
-       numfaces = clmd->clothObject->numfaces;
-       numverts = clmd->clothObject->numverts;
 
-       verts = cloth->verts;
-
-       if ( collmd->bvhtree )
+// return all collision objects in scene
+// collision object will exclude self 
+CollisionModifierData **get_collisionobjects(Scene *scene, Object *self, int *numcollobj)
+{
+       Base *base=NULL;
+       CollisionModifierData **objs = NULL;
+       Object *coll_ob = NULL;
+       CollisionModifierData *collmd = NULL;
+       int numobj = 0, maxobj = 100;
+       
+       objs = MEM_callocN(sizeof(CollisionModifierData *)*maxobj, "CollisionObjectsArray");
+       // check all collision objects
+       for ( base = scene->base.first; base; base = base->next )
        {
-               /* get pointer to bounding volume hierarchy */
-               BVHTree *coll_bvh = collmd->bvhtree;
+               /*Only proceed for mesh object in same layer */
+               if(!(base->object->type==OB_MESH && (base->lay & self->lay))) 
+                       continue;
+               
+               coll_ob = base->object;
+               
+               if(coll_ob == self)
+                               continue;
+               
+               if(coll_ob->pd && coll_ob->pd->deflect)
+               {
+                       collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
+               }
+               else
+                       collmd = NULL;
+               
+               if ( collmd )
+               {       
+                       if(numobj >= maxobj)
+                       {
+                               // realloc
+                               int oldmax = maxobj;
+                               CollisionModifierData **tmp;
+                               maxobj *= 2;
+                               tmp = MEM_callocN(sizeof(CollisionModifierData *)*maxobj, "CollisionObjectsArray");
+                               memcpy(tmp, objs, sizeof(CollisionModifierData *)*oldmax);
+                               MEM_freeN(objs);
+                               objs = tmp;
+                               
+                       }
+                       
+                       objs[numobj] = collmd;
+                       numobj++;
+               }
+               else
+               {
+                       if ( coll_ob->dup_group )
+                       {
+                               GroupObject *go;
+                               Group *group = coll_ob->dup_group;
 
-               /* move object to position (step) in time */
-               collision_move_object ( collmd, step + dt, step );
+                               for ( go= group->gobject.first; go; go= go->next )
+                               {
+                                       coll_ob = go->ob;
+                                       collmd = NULL;
+                                       
+                                       if(coll_ob == self)
+                                               continue;
+                                       
+                                       if(coll_ob->pd && coll_ob->pd->deflect)
+                                       {
+                                               collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
+                                       }
+                                       else
+                                               collmd = NULL;
 
-               /* search for overlapping collision pairs */
-               overlap = BLI_bvhtree_overlap ( cloth_bvh, coll_bvh, &result );
+                                       if ( !collmd )
+                                               continue;
+                                       
+                                       if( !collmd->bvhtree)
+                                               continue;
 
-               collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * result*4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
-               collisions_index = collisions;
+                                       if(numobj >= maxobj)
+                                       {
+                                               // realloc
+                                               int oldmax = maxobj;
+                                               CollisionModifierData **tmp;
+                                               maxobj *= 2;
+                                               tmp = MEM_callocN(sizeof(CollisionModifierData *)*maxobj, "CollisionObjectsArray");
+                                               memcpy(tmp, objs, sizeof(CollisionModifierData *)*oldmax);
+                                               MEM_freeN(objs);
+                                               objs = tmp;
+                                       }
+                                       
+                                       objs[numobj] = collmd;
+                                       numobj++;
+                               }
+                       }
+               }       
+       }
+       *numcollobj = numobj;
+       return objs;
+}
 
-               for ( i = 0; i < result; i++ )
-               {
-                       collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, collisions_index );
-               }
+static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap)
+{
+       int i;
+       
+       *collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * numresult * 4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
+       *collisions_index = *collisions;
 
-               if ( overlap )
-                       MEM_freeN ( overlap );
-       }
-       else
+       for ( i = 0; i < numresult; i++ )
        {
-               if ( G.rt > 0 )
-                       printf ( "cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n" );
+               *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, *collisions_index );
        }
+}
 
+static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
+{
+       Cloth *cloth = clmd->clothObject;
+       int i=0, j = 0, numfaces = 0, numverts = 0;
+       ClothVertex *verts = NULL;
+       int ret = 0;
+       int result = 0;
+       float tnull[3] = {0,0,0};
+       
+       numfaces = clmd->clothObject->numfaces;
+       numverts = clmd->clothObject->numverts;
+       verts = cloth->verts;
+       
        // process all collisions (calculate impulses, TODO: also repulses if distance too short)
        result = 1;
        for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence
@@ -1429,48 +1445,22 @@ int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData
                                        }
                                }
                        }
-
-                       result += cloth_collision_moving ( clmd, collmd, collisions, collisions_index );
-
-                       // apply impulses in parallel
-                       if ( result )
-                       {
-                               for ( i = 0; i < numverts; i++ )
-                               {
-                                       // calculate "velocities" (just xnew = xold + v; no dt in v)
-                                       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;
-
-                                               ret++;
-                                       }
-                               }
-                       }
-
                }
        }
-
-       if ( collisions ) MEM_freeN ( collisions );
-
        return ret;
 }
 
 // cloth - object collisions
-int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
+int cloth_bvh_objcollision (Object *ob, ClothModifierData * clmd, float step, float dt )
 {
-       Base *base=NULL;
-       CollisionModifierData *collmd=NULL;
        Cloth *cloth=NULL;
-       Object *coll_ob=NULL;
        BVHTree *cloth_bvh=NULL;
-       long i=0, j = 0, k = 0, numfaces = 0, numverts = 0;
-       unsigned int result = 0, rounds = 0; // result counts applied collisions; ic is for debug output;
+       int i=0, numfaces = 0, numverts = 0, k, l, j;
+       int rounds = 0; // result counts applied collisions; ic is for debug output;
        ClothVertex *verts = NULL;
        int ret = 0, ret2 = 0;
-       ClothModifierData *tclmd;
-       int collisions = 0, count = 0;
+       CollisionModifierData **collobjs = NULL;
+       int numcollobj = 0;
 
        if ( ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ ) || ! ( ( ( Cloth * ) clmd->clothObject )->bvhtree ) )
        {
@@ -1490,54 +1480,64 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
        // update cloth bvh
        bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
        bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
+       
+       collobjs = get_collisionobjects(clmd->scene, ob, &numcollobj);
+       
+       if(!collobjs)
+               return 0;
 
        do
        {
-               result = 0;
+               CollPair **collisions, **collisions_index;
+               
                ret2 = 0;
 
+               collisions = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
+               collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
+               
                // check all collision objects
-               for ( base = G.scene->base.first; base; base = base->next )
+               for(i = 0; i < numcollobj; i++)
                {
-                       coll_ob = base->object;
-                       collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
-
-                       if ( !collmd )
-                       {
-                               if ( coll_ob->dup_group )
-                               {
-                                       GroupObject *go;
-                                       Group *group = coll_ob->dup_group;
-
-                                       for ( go= group->gobject.first; go; go= go->next )
-                                       {
-                                               coll_ob = go->ob;
-
-                                               collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
-
-                                               if ( !collmd )
-                                                       continue;
-
-                                               tclmd = ( ClothModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Cloth );
-                                               if ( tclmd == clmd )
-                                                       continue;
-
-                                               ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt );
-                                               ret2 += ret;
-                                       }
-                               }
-                       }
-                       else
+                       CollisionModifierData *collmd = collobjs[i];
+                       BVHTreeOverlap *overlap = NULL;
+                       int result = 0;
+                       
+                       if(!collmd->bvhtree)
+                               continue;
+                       
+                       /* move object to position (step) in time */
+                       collision_move_object ( collmd, step + dt, step );
+                       
+                       /* search for overlapping collision pairs */
+                       overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
+                               
+                       // go to next object if no overlap is there
+                       if(!result || !overlap)
                        {
-                               tclmd = ( ClothModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Cloth );
-                               if ( tclmd == clmd )
-                                       continue;
-
-                               ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt );
-                               ret2 += ret;
+                               if ( overlap )
+                                       MEM_freeN ( overlap );
+                               continue;
                        }
+                       
+                       /* check if collisions really happen (costly near check) */
+                       cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], &collisions_index[i], result, overlap);
+                       
+                       // resolve nearby collisions
+                       ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
+                       ret2 += ret;
+                       
+                       if ( overlap )
+                               MEM_freeN ( overlap );
                }
                rounds++;
+               
+               for(i = 0; i < numcollobj; i++)
+               {
+                       if ( collisions[i] ) MEM_freeN ( collisions[i] );
+               }
+                       
+               MEM_freeN(collisions);
+               MEM_freeN(collisions_index);
 
                ////////////////////////////////////////////////////////////
                // update positions
@@ -1558,96 +1558,99 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
                        VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
                }
                ////////////////////////////////////////////////////////////
-
-
+               
+               
                ////////////////////////////////////////////////////////////
                // Test on *simple* selfcollisions
                ////////////////////////////////////////////////////////////
                if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
                {
-
-                       MFace *mface = cloth->mfaces;
-                       BVHTreeOverlap *overlap = NULL;
-
-                       collisions = 1;
-                       verts = cloth->verts; // needed for openMP
-
-                       numfaces = clmd->clothObject->numfaces;
-                       numverts = clmd->clothObject->numverts;
-
-                       verts = cloth->verts;
-
-                       if ( cloth->bvhselftree )
+                       for(l = 0; l < clmd->coll_parms->self_loop_count; l++)
                        {
-                               // search for overlapping collision pairs 
-                               overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
-
-// #pragma omp parallel for private(k, i, j) schedule(static)
-                               for ( k = 0; k < result; k++ )
+                               // TODO: add coll quality rounds again
+                               BVHTreeOverlap *overlap = NULL;
+                               int result = 0;
+       
+                               // collisions = 1;
+                               verts = cloth->verts; // needed for openMP
+       
+                               numfaces = clmd->clothObject->numfaces;
+                               numverts = clmd->clothObject->numverts;
+       
+                               verts = cloth->verts;
+       
+                               if ( cloth->bvhselftree )
                                {
-                                       float temp[3];
-                                       float length = 0;
-                                       float mindistance;
-
-                                       i = overlap[k].indexA;
-                                       j = overlap[k].indexB;
-
-                                       mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
-
-                                       if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
+                                       // search for overlapping collision pairs 
+                                       overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
+       
+       // #pragma omp parallel for private(k, i, j) schedule(static)
+                                       for ( k = 0; k < result; k++ )
                                        {
-                                               if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
-                                                       && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
+                                               float temp[3];
+                                               float length = 0;
+                                               float mindistance;
+       
+                                               i = overlap[k].indexA;
+                                               j = overlap[k].indexB;
+       
+                                               mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
+       
+                                               if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
                                                {
-                                                       continue;
+                                                       if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
+                                                                               && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
+                                                       {
+                                                               continue;
+                                                       }
                                                }
-                                       }
-
-                                       VECSUB ( temp, verts[i].tx, verts[j].tx );
-
-                                       if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
-
-                                       // check for adjacent points (i must be smaller j)
-                                       if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
-                                       {
-                                               continue;
-                                       }
-
-                                       length = Normalize ( temp );
-
-                                       if ( length < mindistance )
-                                       {
-                                               float correction = mindistance - length;
-
-                                               if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
+       
+                                               VECSUB ( temp, verts[i].tx, verts[j].tx );
+       
+                                               if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
+       
+                                               // check for adjacent points (i must be smaller j)
+                                               if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
                                                {
-                                                       VecMulf ( temp, -correction );
-                                                       VECADD ( verts[j].tx, verts[j].tx, temp );
+                                                       continue;
                                                }
-                                               else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
+       
+                                               length = Normalize ( temp );
+       
+                                               if ( length < mindistance )
                                                {
-                                                       VecMulf ( temp, correction );
-                                                       VECADD ( verts[i].tx, verts[i].tx, temp );
+                                                       float correction = mindistance - length;
+       
+                                                       if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
+                                                       {
+                                                               VecMulf ( temp, -correction );
+                                                               VECADD ( verts[j].tx, verts[j].tx, temp );
+                                                       }
+                                                       else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
+                                                       {
+                                                               VecMulf ( temp, correction );
+                                                               VECADD ( verts[i].tx, verts[i].tx, temp );
+                                                       }
+                                                       else
+                                                       {
+                                                               VecMulf ( temp, -correction*0.5 );
+                                                               VECADD ( verts[j].tx, verts[j].tx, temp );
+       
+                                                               VECSUB ( verts[i].tx, verts[i].tx, temp );
+                                                       }
+                                                       ret = 1;
+                                                       ret2 += ret;
                                                }
                                                else
                                                {
-                                                       VecMulf ( temp, -correction*0.5 );
-                                                       VECADD ( verts[j].tx, verts[j].tx, temp );
-
-                                                       VECSUB ( verts[i].tx, verts[i].tx, temp );
+                                                       // check for approximated time collisions
                                                }
-                                               ret = 1;
-                                               ret2 += ret;
-                                       }
-                                       else
-                                       {
-                                               // check for approximated time collisions
                                        }
+       
+                                       if ( overlap )
+                                               MEM_freeN ( overlap );
+       
                                }
-
-                               if ( overlap )
-                                       MEM_freeN ( overlap );
-
                        }
                        ////////////////////////////////////////////////////////////
 
@@ -1668,18 +1671,9 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
                }
        }
        while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
+       
+       if(collobjs)
+               MEM_freeN(collobjs);
 
        return MIN2 ( ret, 1 );
 }
-
-
-/*
-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;
-
-       ret++;
-}
-*/
\ No newline at end of file