diff options
author | Daniel Genrich <daniel.genrich@gmx.net> | 2008-06-03 22:48:54 +0400 |
---|---|---|
committer | Daniel Genrich <daniel.genrich@gmx.net> | 2008-06-03 22:48:54 +0400 |
commit | c84c0201e156915281d1a4d77aaa8802c171007b (patch) | |
tree | e323096b63646a0f932cec917104a0db4997dffc /source/blender/blenkernel | |
parent | 62ca0e07dad7d79c79eda2c3eeef0afd7e939896 (diff) | |
parent | 74903b77f48c9c2ec9c226fc6ae34381d258e47e (diff) |
Collisions: Commit of collision cleanup, put kdop-bvh structure into BLI_kdopbvh (just like kdtree interface now), huge speedup for selfcollisions, also better normal collisions (merge from cloth branch)
Diffstat (limited to 'source/blender/blenkernel')
-rw-r--r-- | source/blender/blenkernel/BKE_cloth.h | 32 | ||||
-rw-r--r-- | source/blender/blenkernel/BKE_collision.h | 90 | ||||
-rw-r--r-- | source/blender/blenkernel/intern/cloth.c | 208 | ||||
-rw-r--r-- | source/blender/blenkernel/intern/collision.c | 1506 | ||||
-rw-r--r-- | source/blender/blenkernel/intern/kdop.c | 860 | ||||
-rw-r--r-- | source/blender/blenkernel/intern/modifier.c | 29 |
6 files changed, 1187 insertions, 1538 deletions
diff --git a/source/blender/blenkernel/BKE_cloth.h b/source/blender/blenkernel/BKE_cloth.h index af920e9762d..2e5da236a89 100644 --- a/source/blender/blenkernel/BKE_cloth.h +++ b/source/blender/blenkernel/BKE_cloth.h @@ -24,14 +24,14 @@ * * The Original Code is: all of this file. * - * Contributor(s): none yet. + * Contributor(s): Daniel Genrich * * ***** END GPL LICENSE BLOCK ***** */ #ifndef BKE_CLOTH_H #define BKE_CLOTH_H -#include "float.h" +#include <float.h> #include "BLI_linklist.h" #include "BKE_customdata.h" @@ -102,7 +102,8 @@ typedef struct Cloth unsigned char old_solver_type; /* unused, only 1 solver here */ unsigned char pad2; short pad3; - struct BVH *tree; /* collision tree for this cloth object */ + struct BVHTree *bvhtree; /* collision tree for this cloth object */ + struct BVHTree *bvhselftree; /* collision tree for this cloth object */ struct MFace *mfaces; struct Implicit_Data *implicit; /* our implicit solver connects to this pointer */ struct Implicit_Data *implicitEM; /* our implicit solver connects to this pointer */ @@ -171,17 +172,10 @@ ClothSpring; /* These are the bits used in SimSettings.flags. */ typedef enum { - //CLOTH_SIMSETTINGS_FLAG_RESET = ( 1 << 1 ), // The CM object requires a reinitializaiton. CLOTH_SIMSETTINGS_FLAG_COLLOBJ = ( 1 << 2 ),// object is only collision object, no cloth simulation is done CLOTH_SIMSETTINGS_FLAG_GOAL = ( 1 << 3 ), // we have goals enabled CLOTH_SIMSETTINGS_FLAG_TEARING = ( 1 << 4 ),// true if tearing is enabled - //CLOTH_SIMSETTINGS_FLAG_CCACHE_PROTECT = ( 1 << 5 ), // true if tearing is enabled - //CLOTH_SIMSETTINGS_FLAG_EDITMODE = ( 1 << 6 ), // are we in editmode? -several things disabled - //CLOTH_SIMSETTINGS_FLAG_CCACHE_FFREE = ( 1 << 7 ), /* force cache freeing */ CLOTH_SIMSETTINGS_FLAG_SCALING = ( 1 << 8 ), /* is advanced scaling active? */ - //CLOTH_SIMSETTINGS_FLAG_LOADED = ( 1 << 9 ), /* did we just got load? */ - //CLOTH_SIMSETTINGS_FLAG_AUTOPROTECT = ( 1 << 10 ), /* is autoprotect enabled? */ - //CLOTH_SIMSETTINGS_FLAG_CCACHE_OUTDATED = (1 << 11), /* while protected, did cache get outdated? */ CLOTH_SIMSETTINGS_FLAG_CCACHE_EDIT = (1 << 12) /* edit cache in editmode */ } CLOTH_SIMSETTINGS_FLAGS; @@ -208,6 +202,7 @@ typedef enum CLOTH_SPRING_FLAG_NEEDED = ( 1 << 2 ), // springs has values to be applied } CLOTH_SPRINGS_FLAGS; + ///////////////////////////////////////////////// // collision.c //////////////////////////////////////////////// @@ -246,7 +241,8 @@ DerivedMesh *clothModifier_do ( ClothModifierData *clmd,Object *ob, DerivedMesh void cloth_update_normals ( ClothVertex *verts, int nVerts, MFace *face, int totface ); // needed for collision.c -void bvh_update_from_cloth ( ClothModifierData *clmd, int moving ); +void bvhtree_update_from_cloth ( ClothModifierData *clmd, int moving ); +void bvhselftree_update_from_cloth ( ClothModifierData *clmd, int moving ); // needed for editmesh.c void cloth_write_cache ( Object *ob, ClothModifierData *clmd, float framenr ); @@ -261,11 +257,6 @@ int cloth_add_spring ( ClothModifierData *clmd, unsigned int indexA, unsigned in //////////////////////////////////////////////// -/* Typedefs for function pointers we need for solvers and collision detection. */ -typedef void ( *CM_COLLISION_SELF ) ( ClothModifierData *clmd, int step ); -typedef void ( *CM_COLLISION_OBJ ) ( ClothModifierData *clmd, int step, CM_COLLISION_RESPONSE collision_response ); - - /* This enum provides the IDs for our solvers. */ // only one available in the moment typedef enum @@ -286,15 +277,6 @@ typedef struct } CM_SOLVER_DEF; -/* used for caching in implicit.c */ -typedef struct Frame -{ - ClothVertex *verts; - ClothSpring *springs; - unsigned int numverts, numsprings; - float time; /* we need float since we want to support sub-frames */ -} -Frame; #endif diff --git a/source/blender/blenkernel/BKE_collision.h b/source/blender/blenkernel/BKE_collision.h index 7328f9108e3..b38bf8662d7 100644 --- a/source/blender/blenkernel/BKE_collision.h +++ b/source/blender/blenkernel/BKE_collision.h @@ -24,7 +24,7 @@ * * The Original Code is: all of this file. * - * Contributor(s): none yet. + * Contributor(s): Daniel Genrich * * ***** END GPL LICENSE BLOCK ***** */ @@ -32,7 +32,7 @@ #define BKE_COLLISIONS_H #include <math.h> -#include "float.h" +#include <float.h> #include <stdlib.h> #include <string.h> @@ -47,68 +47,27 @@ #include "DNA_modifier_types.h" #include "DNA_object_types.h" +#include "BLI_kdopbvh.h" + struct Object; struct Cloth; struct MFace; struct DerivedMesh; struct ClothModifierData; -struct CollisionTree; - //////////////////////////////////////// -// used in kdop.c and collision.c +// used for collisions in collision.c //////////////////////////////////////// -typedef struct CollisionTree -{ - struct CollisionTree *nodes[4]; // 4 children --> quad-tree - struct CollisionTree *parent; - struct CollisionTree *nextLeaf; - struct CollisionTree *prevLeaf; - float bv[26]; // Bounding volume of all nodes / we have 7 axes on a 14-DOP - unsigned int tri_index; // this saves the index of the face - // int point_index[4]; // supports up to 4 points in a leaf - int count_nodes; // how many nodes are used - int traversed; // how many nodes already traversed until this level? - int isleaf; - float alpha; /* for selfcollision */ - float normal[3]; /* for selfcollision */ -} -CollisionTree; -typedef struct BVH +/* COLLISION FLAGS */ +typedef enum { - unsigned int numfaces; - unsigned int numverts; - MVert *current_x; // e.g. txold in clothvertex - MVert *current_xold; // e.g. tx in clothvertex - MFace *mfaces; // just a pointer to the original datastructure - struct LinkNode *tree; - CollisionTree *root; // TODO: saving the root --> is this really needed? YES! - CollisionTree *leaf_tree; /* Tail of the leaf linked list. */ - CollisionTree *leaf_root; /* Head of the leaf linked list. */ - float epsilon; /* epslion is used for inflation of the k-dop */ - int flags; /* bvhFlags */ -} -BVH; -//////////////////////////////////////// + COLLISION_IN_FUTURE = ( 1 << 1 ), +} COLLISION_FLAGS; - -//////////////////////////////////////// -// kdop.c //////////////////////////////////////// - -// needed for collision.c -typedef void ( *CM_COLLISION_RESPONSE ) ( ModifierData *md1, ModifierData *md2, CollisionTree *tree1, CollisionTree *tree2 ); - -// needed for collision.c -int bvh_traverse ( ModifierData * md1, ModifierData * md2, CollisionTree * tree1, CollisionTree * tree2, float step, CM_COLLISION_RESPONSE collision_response, int selfcollision); - -//////////////////////////////////////// - - -//////////////////////////////////////// -// used for collisions in kdop.c and also collision.c +// used for collisions in collision.c //////////////////////////////////////// /* used for collisions in collision.c */ typedef struct CollPair @@ -119,10 +78,10 @@ typedef struct CollPair float normal[3]; float vector[3]; // unnormalized collision vector: p2-p1 float pa[3], pb[3]; // collision point p1 on face1, p2 on face2 - int lastsign; // indicates if the distance sign has changed, unused itm + int flag; float time; // collision time, from 0 up to 1 - unsigned int ap1, ap2, ap3, bp1, bp2, bp3; - unsigned int pointsb[4]; + int ap1, ap2, ap3, bp1, bp2, bp3; + int pointsb[4]; } CollPair; @@ -157,32 +116,15 @@ FaceCollPair; // forward declarations ///////////////////////////////////////////////// -// NOTICE: mvert-routines for building + update the BVH are the most native ones - -// builds bounding volume hierarchy -void bvh_build (BVH *bvh); -BVH *bvh_build_from_mvert (MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int numverts, float epsilon); - -// frees the same -void bvh_free ( BVH * bvh ); - -// checks two bounding volume hierarchies for potential collisions and returns some list with those - - -// update bounding volumes, needs updated positions in bvh->current_xold (static) -// and also bvh->current_x if moving==1 -void bvh_update_from_mvert(BVH * bvh, MVert *x, unsigned int numverts, MVert *xnew, int moving); -void bvh_update(BVH * bvh, int moving); - LinkNode *BLI_linklist_append_fast ( LinkNode **listp, void *ptr ); // move Collision modifier object inter-frame with step = [0,1] // defined in collisions.c -void collision_move_object(CollisionModifierData *collmd, float step, float prevstep); +void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep ); // interface for collision functions -void collisions_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3); -void interpolateOnTriangle(float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3); +void collisions_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 ); +void interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 ); ///////////////////////////////////////////////// diff --git a/source/blender/blenkernel/intern/cloth.c b/source/blender/blenkernel/intern/cloth.c index 09a51bb37a4..6034b85e20f 100644 --- a/source/blender/blenkernel/intern/cloth.c +++ b/source/blender/blenkernel/intern/cloth.c @@ -45,6 +45,8 @@ #include "BKE_pointcache.h" +#include "BLI_kdopbvh.h" + #ifdef _WIN32 void tstart ( void ) {} @@ -151,13 +153,14 @@ void cloth_init ( ClothModifierData *clmd ) clmd->sim_parms->goalfrict = 0.0f; } - -BVH *bvh_build_from_cloth (ClothModifierData *clmd, float epsilon) +BVHTree *bvhselftree_build_from_cloth (ClothModifierData *clmd, float epsilon) { - unsigned int i = 0; - BVH *bvh=NULL; + int i; + BVHTree *bvhtree; Cloth *cloth = clmd->clothObject; - ClothVertex *verts = NULL; + ClothVertex *verts; + MFace *mfaces; + float co[12]; if(!clmd) return NULL; @@ -168,69 +171,171 @@ BVH *bvh_build_from_cloth (ClothModifierData *clmd, float epsilon) return NULL; verts = cloth->verts; + mfaces = cloth->mfaces; // in the moment, return zero if no faces there - if(!cloth->numfaces) + if(!cloth->numverts) return NULL; - bvh = MEM_callocN(sizeof(BVH), "BVH"); - if (bvh == NULL) + // create quadtree with k=26 + bvhtree = BLI_bvhtree_new(cloth->numverts, epsilon, 4, 6); + + // fill tree + for(i = 0; i < cloth->numverts; i++, verts++) { - printf("bvh: Out of memory.\n"); - return NULL; + VECCOPY(&co[0*3], verts->xold); + + BLI_bvhtree_insert(bvhtree, i, co, 1); } - // springs = cloth->springs; - // numsprings = cloth->numsprings; + // balance tree + BLI_bvhtree_balance(bvhtree); + + return bvhtree; +} - bvh->epsilon = epsilon; - bvh->numfaces = cloth->numfaces; - bvh->mfaces = cloth->mfaces; +BVHTree *bvhtree_build_from_cloth (ClothModifierData *clmd, float epsilon) +{ + int i; + BVHTree *bvhtree; + Cloth *cloth = clmd->clothObject; + ClothVertex *verts; + MFace *mfaces; + float co[12]; + + if(!clmd) + return NULL; - bvh->numverts = cloth->numverts; + cloth = clmd->clothObject; + + if(!cloth) + return NULL; - bvh->current_x = MEM_callocN ( sizeof ( MVert ) * bvh->numverts, "bvh->current_x" ); + verts = cloth->verts; + mfaces = cloth->mfaces; - if (bvh->current_x == NULL) - { - printf("bvh: Out of memory.\n"); - MEM_freeN(bvh); + // in the moment, return zero if no faces there + if(!cloth->numfaces) return NULL; - } - for(i = 0; i < bvh->numverts; i++) + // create quadtree with k=26 + bvhtree = BLI_bvhtree_new(cloth->numfaces, epsilon, 4, 26); + + // fill tree + for(i = 0; i < cloth->numfaces; i++, mfaces++) { - VECCOPY(bvh->current_x[i].co, verts[i].tx); + VECCOPY(&co[0*3], verts[mfaces->v1].xold); + VECCOPY(&co[1*3], verts[mfaces->v2].xold); + VECCOPY(&co[2*3], verts[mfaces->v3].xold); + + if(mfaces->v4) + VECCOPY(&co[3*3], verts[mfaces->v4].xold); + + BLI_bvhtree_insert(bvhtree, i, co, (mfaces->v4 ? 4 : 3)); } - bvh_build (bvh); + // balance tree + BLI_bvhtree_balance(bvhtree); - return bvh; + return bvhtree; } -void bvh_update_from_cloth(ClothModifierData *clmd, int moving) -{ +void bvhtree_update_from_cloth(ClothModifierData *clmd, int moving) +{ unsigned int i = 0; Cloth *cloth = clmd->clothObject; - BVH *bvh = cloth->tree; + BVHTree *bvhtree = cloth->bvhtree; ClothVertex *verts = cloth->verts; + MFace *mfaces; + float co[12], co_moving[12]; + int ret = 0; - if(!bvh) + if(!bvhtree) return; - if(cloth->numverts!=bvh->numverts) - return; + mfaces = cloth->mfaces; - if(cloth->verts) + // update vertex position in bvh tree + if(verts && mfaces) { - for(i = 0; i < bvh->numverts; i++) + for(i = 0; i < cloth->numfaces; i++, mfaces++) { - VECCOPY(bvh->current_x[i].co, verts[i].tx); - VECCOPY(bvh->current_xold[i].co, verts[i].txold); + VECCOPY(&co[0*3], verts[mfaces->v1].txold); + VECCOPY(&co[1*3], verts[mfaces->v2].txold); + VECCOPY(&co[2*3], verts[mfaces->v3].txold); + + if(mfaces->v4) + VECCOPY(&co[3*3], verts[mfaces->v4].txold); + + // copy new locations into array + if(moving) + { + // update moving positions + VECCOPY(&co_moving[0*3], verts[mfaces->v1].tx); + VECCOPY(&co_moving[1*3], verts[mfaces->v2].tx); + VECCOPY(&co_moving[2*3], verts[mfaces->v3].tx); + + if(mfaces->v4) + VECCOPY(&co_moving[3*3], verts[mfaces->v4].tx); + + ret = BLI_bvhtree_update_node(bvhtree, i, co, co_moving, (mfaces->v4 ? 4 : 3)); + } + else + { + ret = BLI_bvhtree_update_node(bvhtree, i, co, NULL, (mfaces->v4 ? 4 : 3)); + } + + // check if tree is already full + if(!ret) + break; } + + BLI_bvhtree_update_tree(bvhtree); } +} + +void bvhselftree_update_from_cloth(ClothModifierData *clmd, int moving) +{ + unsigned int i = 0; + Cloth *cloth = clmd->clothObject; + BVHTree *bvhtree = cloth->bvhselftree; + ClothVertex *verts = cloth->verts; + MFace *mfaces; + float co[12], co_moving[12]; + int ret = 0; + + if(!bvhtree) + return; - bvh_update(bvh, moving); + mfaces = cloth->mfaces; + + // update vertex position in bvh tree + if(verts && mfaces) + { + for(i = 0; i < cloth->numverts; i++, verts++) + { + VECCOPY(&co[0*3], verts->txold); + + // copy new locations into array + if(moving) + { + // update moving positions + VECCOPY(&co_moving[0*3], verts->tx); + + ret = BLI_bvhtree_update_node(bvhtree, i, co, co_moving, 1); + } + else + { + ret = BLI_bvhtree_update_node(bvhtree, i, co, NULL, 1); + } + + // check if tree is already full + if(!ret) + break; + } + + BLI_bvhtree_update_tree(bvhtree); + } } int modifiers_indexInObject(Object *ob, ModifierData *md_seek); @@ -541,8 +646,11 @@ void cloth_free_modifier ( Object *ob, ClothModifierData *clmd ) cloth->numsprings = 0; // free BVH collision tree - if ( cloth->tree ) - bvh_free ( ( BVH * ) cloth->tree ); + if ( cloth->bvhtree ) + BLI_bvhtree_free ( cloth->bvhtree ); + + if ( cloth->bvhselftree ) + BLI_bvhtree_free ( cloth->bvhselftree ); // we save our faces for collision objects if ( cloth->mfaces ) @@ -611,8 +719,11 @@ void cloth_free_modifier_extern ( ClothModifierData *clmd ) cloth->numsprings = 0; // free BVH collision tree - if ( cloth->tree ) - bvh_free ( ( BVH * ) cloth->tree ); + if ( cloth->bvhtree ) + BLI_bvhtree_free ( cloth->bvhtree ); + + if ( cloth->bvhselftree ) + BLI_bvhtree_free ( cloth->bvhselftree ); // we save our faces for collision objects if ( cloth->mfaces ) @@ -751,6 +862,7 @@ static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *d ClothVertex *verts = NULL; float tnull[3] = {0,0,0}; Cloth *cloth = NULL; + float maxdist = 0; // If we have a clothObject, free it. if ( clmd->clothObject != NULL ) @@ -810,6 +922,7 @@ static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *d VECCOPY ( verts->xold, verts->x ); VECCOPY ( verts->xconst, verts->x ); VECCOPY ( verts->txold, verts->x ); + VECCOPY ( verts->tx, verts->x ); VecMulf ( verts->v, 0.0f ); verts->impulse_count = 0; @@ -819,8 +932,7 @@ static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *d // apply / set vertex groups // has to be happen before springs are build! cloth_apply_vgroup (clmd, dm); - - + if ( !cloth_build_springs ( clmd, dm ) ) { cloth_free_modifier ( ob, clmd ); @@ -845,12 +957,18 @@ static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *d if(!first) implicit_set_positions(clmd); - clmd->clothObject->tree = bvh_build_from_cloth ( clmd, clmd->coll_parms->epsilon ); + clmd->clothObject->bvhtree = bvhtree_build_from_cloth ( clmd, clmd->coll_parms->epsilon ); + + for(i = 0; i < dm->getNumVerts(dm); i++) + { + maxdist = MAX2(maxdist, clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len*2.0)); + } + + clmd->clothObject->bvhselftree = bvhselftree_build_from_cloth ( clmd, maxdist ); return 1; } - static void cloth_from_mesh ( Object *ob, ClothModifierData *clmd, DerivedMesh *dm ) { unsigned int numverts = dm->getNumVerts ( dm ); diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c index e244ccca306..36cc37eab44 100644 --- a/source/blender/blenkernel/intern/collision.c +++ b/source/blender/blenkernel/intern/collision.c @@ -41,7 +41,6 @@ #include "BKE_global.h" #include "BKE_mesh.h" #include "BKE_object.h" -#include "BKE_cloth.h" #include "BKE_modifier.h" #include "BKE_utildefines.h" #include "BKE_DerivedMesh.h" @@ -49,6 +48,38 @@ #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 ***********************************/ @@ -66,58 +97,80 @@ void collision_move_object ( CollisionModifierData *collmd, float step, float pr VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step ); VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co ); } - bvh_update_from_mvert ( collmd->bvh, collmd->current_x, collmd->numverts, collmd->current_xnew, 1 ); + bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 ); } -/* build bounding volume hierarchy from mverts (see kdop.c for whole BVH code) */ -BVH *bvh_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int numverts, float epsilon ) +BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int numverts, float epsilon ) { - BVH *bvh=NULL; - - bvh = MEM_callocN ( sizeof ( BVH ), "BVH" ); - if ( bvh == NULL ) - { - printf ( "bvh: Out of memory.\n" ); - return NULL; - } - - // in the moment, return zero if no faces there - if ( !numfaces ) - return NULL; + BVHTree *tree; + float co[12]; + int i; + MFace *tface = mfaces; - bvh->epsilon = epsilon; - bvh->numfaces = numfaces; - bvh->mfaces = mfaces; + tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 ); - // we have no faces, we save seperate points - if ( !mfaces ) + // fill tree + for ( i = 0; i < numfaces; i++, tface++ ) { - bvh->numfaces = numverts; - } + VECCOPY ( &co[0*3], x[tface->v1].co ); + VECCOPY ( &co[1*3], x[tface->v2].co ); + VECCOPY ( &co[2*3], x[tface->v3].co ); + if ( tface->v4 ) + VECCOPY ( &co[3*3], x[tface->v4].co ); - bvh->numverts = numverts; - bvh->current_x = MEM_dupallocN ( x ); + BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) ); + } - bvh_build ( bvh ); + // balance tree + BLI_bvhtree_balance ( tree ); - return bvh; + return tree; } -void bvh_update_from_mvert ( BVH * bvh, MVert *x, unsigned int numverts, MVert *xnew, int moving ) +void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int numverts, int moving ) { - if ( !bvh ) - return; + int i; + MFace *mfaces = faces; + float co[12], co_moving[12]; + int ret = 0; - if ( numverts!=bvh->numverts ) + if ( !bvhtree ) return; if ( x ) - memcpy ( bvh->current_xold, x, sizeof ( MVert ) * numverts ); + { + for ( i = 0; i < numfaces; i++, mfaces++ ) + { + VECCOPY ( &co[0*3], x[mfaces->v1].co ); + VECCOPY ( &co[1*3], x[mfaces->v2].co ); + VECCOPY ( &co[2*3], x[mfaces->v3].co ); + if ( mfaces->v4 ) + VECCOPY ( &co[3*3], x[mfaces->v4].co ); + + // copy new locations into array + if ( moving && xnew ) + { + // update moving positions + VECCOPY ( &co_moving[0*3], xnew[mfaces->v1].co ); + VECCOPY ( &co_moving[1*3], xnew[mfaces->v2].co ); + VECCOPY ( &co_moving[2*3], xnew[mfaces->v3].co ); + if ( mfaces->v4 ) + VECCOPY ( &co_moving[3*3], xnew[mfaces->v4].co ); + + ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) ); + } + else + { + ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) ); + } - if ( xnew ) - memcpy ( bvh->current_x, xnew, sizeof ( MVert ) * numverts ); + // check if tree is already full + if ( !ret ) + break; + } - bvh_update ( bvh, moving ); + BLI_bvhtree_update_tree ( bvhtree ); + } } /*********************************** @@ -125,47 +178,48 @@ Collision modifier code end ***********************************/ /** - * gsl_poly_solve_cubic - - * - * copied from SOLVE_CUBIC.C --> GSL - */ +* gsl_poly_solve_cubic - +* +* copied from SOLVE_CUBIC.C --> GSL +*/ -/* DG: debug hint! don't forget that all functions were "fabs", "sinf", etc before */ -#define mySWAP(a,b) { float tmp = b ; b = a ; a = tmp ; } +#define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0) -int gsl_poly_solve_cubic ( float a, float b, float c, float *x0, float *x1, float *x2 ) +int +gsl_poly_solve_cubic (double a, double b, double c, + double *x0, double *x1, double *x2) { - float q = ( a * a - 3 * b ); - float r = ( 2 * a * a * a - 9 * a * b + 27 * c ); + double q = (a * a - 3 * b); + double r = (2 * a * a * a - 9 * a * b + 27 * c); - float Q = q / 9; - float R = r / 54; + double Q = q / 9; + double R = r / 54; - float Q3 = Q * Q * Q; - float R2 = R * R; + double Q3 = Q * Q * Q; + double R2 = R * R; - float CR2 = 729 * r * r; - float CQ3 = 2916 * q * q * q; + double CR2 = 729 * r * r; + double CQ3 = 2916 * q * q * q; - if ( R == 0 && Q == 0 ) + if (R == 0 && Q == 0) { *x0 = - a / 3 ; *x1 = - a / 3 ; *x2 = - a / 3 ; return 3 ; } - else if ( CR2 == CQ3 ) + else if (CR2 == CQ3) { /* this test is actually R2 == Q3, written in a form suitable - for exact computation with integers */ + 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. */ + /* Due to finite precision some double roots may be missed, and + considered to be a pair of complex roots z = x +/- epsilon i + close to the real axis. */ - float sqrtQ = sqrt ( Q ); + double sqrtQ = sqrt (Q); - if ( R > 0 ) + if (R > 0) { *x0 = -2 * sqrtQ - a / 3; *x1 = sqrtQ - a / 3; @@ -179,72 +233,88 @@ int gsl_poly_solve_cubic ( float a, float b, float c, float *x0, float *x1, floa } return 3 ; } - else if ( CR2 < CQ3 ) /* equivalent to R2 < Q3 */ + else if (CR2 < CQ3) /* equivalent to R2 < Q3 */ { - float sqrtQ = sqrt ( Q ); - float sqrtQ3 = sqrtQ * sqrtQ * sqrtQ; - float theta = acos ( R / sqrtQ3 ); - float norm = -2 * sqrtQ; - *x0 = norm * cos ( theta / 3 ) - a / 3; - *x1 = norm * cos ( ( theta + 2.0 * M_PI ) / 3 ) - a / 3; - *x2 = norm * cos ( ( theta - 2.0 * M_PI ) / 3 ) - a / 3; + double sqrtQ = sqrt (Q); + double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ; + double theta = acos (R / sqrtQ3); + double norm = -2 * sqrtQ; + *x0 = norm * cos (theta / 3) - a / 3; + *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3; + *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3; /* Sort *x0, *x1, *x2 into increasing order */ - if ( *x0 > *x1 ) - mySWAP ( *x0, *x1 ) ; + if (*x0 > *x1) + mySWAP(*x0, *x1) ; - if ( *x1 > *x2 ) + if (*x1 > *x2) { - mySWAP ( *x1, *x2 ) ; + mySWAP(*x1, *x2) ; - if ( *x0 > *x1 ) - mySWAP ( *x0, *x1 ) ; + if (*x0 > *x1) + mySWAP(*x0, *x1) ; } return 3; } else { - float sgnR = ( R >= 0 ? 1 : -1 ); - float A = -sgnR * pow ( ABS ( R ) + sqrt ( R2 - Q3 ), 1.0/3.0 ); - float B = Q / A ; + double sgnR = (R >= 0 ? 1 : -1); + double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0); + double B = Q / A ; *x0 = A + B - a / 3; return 1; } } + /** - * gsl_poly_solve_quadratic - * - * copied from GSL - */ -int gsl_poly_solve_quadratic ( float a, float b, float c, float *x0, float *x1 ) +* gsl_poly_solve_quadratic +* +* copied from GSL +*/ +int +gsl_poly_solve_quadratic (double a, double b, double c, + double *x0, double *x1) { - float disc = b * b - 4 * a * c; + double disc = b * b - 4 * a * c; + + if (a == 0) /* Handle linear case */ + { + if (b == 0) + { + return 0; + } + else + { + *x0 = -c / b; + return 1; + }; + } - if ( disc > 0 ) + if (disc > 0) { - if ( b == 0 ) + if (b == 0) { - float r = ABS ( 0.5 * sqrt ( disc ) / a ); + double r = fabs (0.5 * sqrt (disc) / a); *x0 = -r; *x1 = r; } else { - float sgnb = ( b > 0 ? 1 : -1 ); - float temp = -0.5 * ( b + sgnb * sqrt ( disc ) ); - float r1 = temp / a ; - float r2 = c / temp ; + double sgnb = (b > 0 ? 1 : -1); + double temp = -0.5 * (b + sgnb * sqrt (disc)); + double r1 = temp / a ; + double r2 = c / temp ; - if ( r1 < r2 ) + if (r1 < r2) { *x0 = r1 ; *x1 = r2 ; - } - else + } + else { *x0 = r2 ; *x1 = r1 ; @@ -252,7 +322,7 @@ int gsl_poly_solve_quadratic ( float a, float b, float c, float *x0, float *x1 } return 2; } - else if ( disc == 0 ) + else if (disc == 0) { *x0 = -0.5 * b / a ; *x1 = -0.5 * b / a ; @@ -266,79 +336,88 @@ int gsl_poly_solve_quadratic ( float a, float b, float c, float *x0, float *x1 -/* - * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation" - * page 4, left column - */ -int cloth_get_collision_time ( float a[3], float b[3], float c[3], float d[3], float e[3], float f[3], float solution[3] ) +/* +* 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] ) { 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]; - + // x^0 - checked + double g = a[0] * c[1] * e[2] - a[0] * c[2] * e[1] + + a[1] * c[2] * e[0] - a[1] * c[0] * e[2] + + a[2] * c[0] * e[1] - a[2] * c[1] * e[0]; + + // x^1 + double 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]; + + // x^2 + double 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]; + + // x^3 - checked + double 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]; + + /* + printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]); + printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]); + printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]); + + printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]); + printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]); + printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]); + + printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]); + printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]); + 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 ) > ALMOST_ZERO ) + if ( ABS ( j ) > DBL_EPSILON ) { i /= j; h /= j; g /= j; - num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] ); } - else if ( ABS ( i ) > ALMOST_ZERO ) + else { num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] ); solution[2] = -1.0; } - else if ( ABS ( h ) > ALMOST_ZERO ) - { - solution[0] = -g / h; - solution[1] = solution[2] = -1.0; - num_sols = 1; - } - else if ( ABS ( g ) > ALMOST_ZERO ) - { - solution[0] = 0; - solution[1] = solution[2] = -1.0; - num_sols = 1; - } + + // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0], solution[1], solution[2]); // Discard negative solutions - if ( ( num_sols >= 1 ) && ( solution[0] < 0 ) ) + if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) ) { --num_sols; solution[0] = solution[num_sols]; } - if ( ( num_sols >= 2 ) && ( solution[1] < 0 ) ) + if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) ) { --num_sols; solution[1] = solution[num_sols]; } - if ( ( num_sols == 3 ) && ( solution[2] < 0 ) ) + if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) ) { --num_sols; } @@ -374,6 +453,7 @@ int cloth_get_collision_time ( float a[3], float b[3], float c[3], float d[3], f 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 ) { @@ -419,38 +499,37 @@ DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float VECADDMUL ( to, v3, w3 ); } -int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd ) + +int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end ) { int result = 0; - LinkNode *search = NULL; - CollPair *collpair = NULL; Cloth *cloth1; float w1, w2, w3, u1, u2, u3; float v1[3], v2[3], relativeVelocity[3]; float magrelVel; - float epsilon2 = collmd->bvh->epsilon; + float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); cloth1 = clmd->clothObject; - search = clmd->coll_parms->collision_list; - - while ( search ) + for ( ; collpair != collision_end; collpair++ ) { - collpair = search->link; + // only handle static collisions here + if ( collpair->flag & COLLISION_IN_FUTURE ) + continue; // compute barycentric coordinates for both collision points collision_compute_barycentric ( collpair->pa, - cloth1->verts[collpair->ap1].txold, - cloth1->verts[collpair->ap2].txold, - cloth1->verts[collpair->ap3].txold, - &w1, &w2, &w3 ); + cloth1->verts[collpair->ap1].txold, + cloth1->verts[collpair->ap2].txold, + cloth1->verts[collpair->ap3].txold, + &w1, &w2, &w3 ); // 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 ); + collmd->current_x[collpair->bp1].co, + collmd->current_x[collpair->bp2].co, + collmd->current_x[collpair->bp3].co, + &u1, &u2, &u3 ); // Calculate relative "velocity". collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 ); @@ -530,70 +609,50 @@ int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifier result = 1; } - - search = search->next; } - - return result; } -int cloth_collision_response_moving_tris ( ClothModifierData *clmd, ClothModifierData *coll_clmd ) -{ - return 1; -} - - -int cloth_collision_response_moving_edges ( ClothModifierData *clmd, ClothModifierData *coll_clmd ) -{ - return 1; -} - -void cloth_collision_static ( ModifierData *md1, ModifierData *md2, CollisionTree *tree1, CollisionTree *tree2 ) +//Determines collisions on overlap, collisions are writen to collpair[i] and collision+number_collision_found is returned +CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, CollPair *collpair ) { ClothModifierData *clmd = ( ClothModifierData * ) md1; CollisionModifierData *collmd = ( CollisionModifierData * ) md2; - CollPair *collpair = NULL; - Cloth *cloth1=NULL; - MFace *face1=NULL, *face2=NULL; - ClothVertex *verts1=NULL; + MFace *face1=NULL, *face2 = NULL; + ClothVertex *verts1 = clmd->clothObject->verts; double distance = 0; - float epsilon = clmd->coll_parms->epsilon; - float epsilon2 = ( ( CollisionModifierData * ) md2 )->bvh->epsilon; - unsigned int i = 0; + float epsilon1 = clmd->coll_parms->epsilon; + float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); + int i; + + face1 = & ( clmd->clothObject->mfaces[overlap->indexA] ); + face2 = & ( collmd->mfaces[overlap->indexB] ); + // check all 4 possible collisions for ( i = 0; i < 4; i++ ) { - collpair = ( CollPair * ) MEM_callocN ( sizeof ( CollPair ), "cloth coll pair" ); - - cloth1 = clmd->clothObject; - - verts1 = cloth1->verts; - - face1 = & ( cloth1->mfaces[tree1->tri_index] ); - face2 = & ( collmd->mfaces[tree2->tri_index] ); - - // check all possible pairs of triangles if ( i == 0 ) { + // fill faceA collpair->ap1 = face1->v1; collpair->ap2 = face1->v2; collpair->ap3 = face1->v3; + // fill faceB collpair->bp1 = face2->v1; collpair->bp2 = face2->v2; collpair->bp3 = face2->v3; - } - - if ( i == 1 ) + else if ( i == 1 ) { if ( face1->v4 ) { - collpair->ap1 = face1->v3; + // fill faceA + collpair->ap1 = face1->v1; collpair->ap2 = face1->v4; - collpair->ap3 = face1->v1; + collpair->ap3 = face1->v3; + // fill faceB collpair->bp1 = face2->v1; collpair->bp2 = face2->v2; collpair->bp3 = face2->v3; @@ -601,386 +660,747 @@ void cloth_collision_static ( ModifierData *md1, ModifierData *md2, CollisionTre else i++; } - if ( i == 2 ) { if ( face2->v4 ) { + // fill faceA collpair->ap1 = face1->v1; collpair->ap2 = face1->v2; collpair->ap3 = face1->v3; - collpair->bp1 = face2->v3; + // fill faceB + collpair->bp1 = face2->v1; collpair->bp2 = face2->v4; - collpair->bp3 = face2->v1; + collpair->bp3 = face2->v3; } else - i+=2; + break; } - - if ( i == 3 ) + else if ( i == 3 ) { - if ( ( face1->v4 ) && ( face2->v4 ) ) + if ( face1->v4 && face2->v4 ) { - collpair->ap1 = face1->v3; + // fill faceA + collpair->ap1 = face1->v1; collpair->ap2 = face1->v4; - collpair->ap3 = face1->v1; + collpair->ap3 = face1->v3; - collpair->bp1 = face2->v3; + // fill faceB + collpair->bp1 = face2->v1; collpair->bp2 = face2->v4; - collpair->bp3 = face2->v1; + collpair->bp3 = face2->v3; } else - i++; + break; } - // calc SIPcode (?) - - if ( i < 4 ) - { - // calc distance + normal #ifdef WITH_BULLET - 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 ); + // 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 ); #else - // just be sure that we don't add anything - distance = 2.0 * ( epsilon + epsilon2 + ALMOST_ZERO ); + // just be sure that we don't add anything + distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO ); #endif - if ( distance <= ( epsilon + epsilon2 + ALMOST_ZERO ) ) - { - // printf("dist: %f\n", (float)distance); - // collpair->face1 = tree1->tri_index; - // collpair->face2 = tree2->tri_index; + if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) ) + { + VECCOPY ( collpair->normal, collpair->vector ); + Normalize ( collpair->normal ); - VECCOPY ( collpair->normal, collpair->vector ); - Normalize ( collpair->normal ); + collpair->distance = distance; + collpair->flag = 0; + } + else + { + float w1, w2, w3, u1, u2, u3; + float v1[3], v2[3], relativeVelocity[3]; - collpair->distance = distance; - BLI_linklist_prepend ( &clmd->coll_parms->collision_list, collpair ); + // 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 ); - } - else + // 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 ); + + // Calculate relative "velocity". + collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 ); + + 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 ); + + if(sqrt(INPR(relativeVelocity, relativeVelocity)) >= distance) { - MEM_freeN ( collpair ); + // check for collision in the future + collpair->flag |= COLLISION_IN_FUTURE; } } - else - { - MEM_freeN ( collpair ); - } + collpair++; } + return collpair; } -int cloth_are_edges_adjacent ( ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair ) +int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end ) { - Cloth *cloth1 = NULL, *cloth2 = NULL; - ClothVertex *verts1 = NULL, *verts2 = NULL; - float temp[3]; + 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; - cloth2 = coll_clmd->clothObject; - verts1 = cloth1->verts; - verts2 = cloth2->verts; + for ( ; collpair != collision_end; collpair++ ) + { + // compute barycentric coordinates for both collision points + collision_compute_barycentric ( collpair->pa, + cloth1->verts[collpair->ap1].txold, + cloth1->verts[collpair->ap2].txold, + cloth1->verts[collpair->ap3].txold, + &w1, &w2, &w3 ); - VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold ); - 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].xold, verts2[edgecollpair->p22].xold ); - if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO ) - return 1; + // Calculate relative "velocity". + collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 ); - VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold ); - 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 ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold ); - if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO ) - return 1; + VECSUB ( relativeVelocity, v2, v1 ); - return 0; + // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). + magrelVel = INPR ( relativeVelocity, collpair->normal ); + + // printf("magrelVel: %f\n", magrelVel); + + // Calculate masses of points. + // TODO + + // 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. + float magtangent = 0, repulse = 0, d = 0; + double impulse = 0.0; + float vrel_t_pre[3]; + float temp[3]; + + // calculate tangential velocity + VECCOPY ( temp, collpair->normal ); + VecMulf ( temp, magrelVel ); + VECSUB ( vrel_t_pre, relativeVelocity, temp ); + + // Decrease in magnitude of relative tangential velocity due to coulomb friction + // in original formula "magrelVel" should be the "change of relative velocity in normal direction" + magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) ); + + // Apply friction impulse. + if ( magtangent > ALMOST_ZERO ) + { + Normalize ( vrel_t_pre ); + + impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); + 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 ); + } + + // Apply velocity stopping impulse + // I_c = m * v_N / 2.0 + // no 2.0 * magrelVel normally, but looks nicer DG + impulse = magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); + + VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse ); + cloth1->verts[collpair->ap1].impulse_count++; + + VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse ); + cloth1->verts[collpair->ap2].impulse_count++; + + VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse ); + cloth1->verts[collpair->ap3].impulse_count++; + + // Apply repulse impulse if distance too short + // I_r = -min(dt*kd, m(0,1d/dt - v_n)) + /* + d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance; + if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) ) + { + repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel ); + + // stay on the safe side and clamp repulse + if ( impulse > ALMOST_ZERO ) + repulse = MIN2 ( repulse, 5.0*impulse ); + repulse = MAX2 ( impulse, repulse ); + + impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25 + VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, impulse ); + VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, impulse ); + VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, impulse ); + } + */ + result = 1; + } + } + return result; } -void cloth_collision_moving_edges ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 ) +static float projectPointOntoLine(float *p, float *a, float *b) { - EdgeCollPair edgecollpair; - Cloth *cloth1=NULL, *cloth2=NULL; - MFace *face1=NULL, *face2=NULL; - ClothVertex *verts1=NULL, *verts2=NULL; - unsigned int i = 0, j = 0, k = 0; - int numsolutions = 0; - float a[3], b[3], c[3], d[3], e[3], f[3], solution[3]; + float ba[3], pa[3]; + VECSUB(ba, b, a); + VECSUB(pa, p, a); + return INPR(pa, ba) / INPR(ba, ba); +} - cloth1 = clmd->clothObject; - cloth2 = coll_clmd->clothObject; +static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal) +{ + float line1[3], line2[3]; + float length; - verts1 = cloth1->verts; - verts2 = cloth2->verts; + 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); - face1 = & ( cloth1->mfaces[tree1->tri_index] ); - face2 = & ( cloth2->mfaces[tree2->tri_index] ); + - for ( i = 0; i < 5; i++ ) + length = Normalize(out_normal); + if (length <= FLT_EPSILON) + { // lines are collinear + VECSUB(out_normal, np2, np1); + Normalize(out_normal); + } +} + +static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2) +{ + float temp[3], temp2[3]; + + double a, b, c, e, f; + + VECSUB(temp, x2, x1); + a = INPR(temp, temp); + + VECSUB(temp2, x4, x3); + b = -INPR(temp, temp2); + + c = INPR(temp2, temp2); + + VECSUB(temp2, x3, x1); + e = INPR(temp, temp2); + + VECSUB(temp, x4, x3); + f = -INPR(temp, temp2); + + *w1 = (e * c - b * f) / (a * c - b * b); + *w2 = (f - b * *w1) / c; + +} + +// 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) +{ + float line1[3], line2[3], cross[3]; + float length; + float temp[3], temp2[3]; + float dist_a1, dist_a2; + + VECSUB(line1, np12, np11); + VECSUB(line2, np22, np21); + + Crossf(cross, line1, line2); + length = INPR(cross, cross); + + if (length < FLT_EPSILON) { - if ( i == 0 ) + *out_a2 = projectPointOntoLine(np11, np21, np22); + if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON)) { - edgecollpair.p11 = face1->v1; - edgecollpair.p12 = face1->v2; + *out_a1 = 0; + calculateEENormal(np11, np12, np21, np22, out_normal); + VECSUB(temp, np22, np21); + VecMulf(temp, *out_a2); + VECADD(temp2, temp, np21); + VECADD(temp2, temp2, np11); + return INPR(temp2, temp2); } - else if ( i == 1 ) - { - edgecollpair.p11 = face1->v2; - edgecollpair.p12 = face1->v3; - } - else if ( i == 2 ) - { - if ( face1->v4 ) + + CLAMP(*out_a2, 0.0, 1.0); + if (*out_a2 > .5) + { // == 1.0 + *out_a1 = projectPointOntoLine(np22, np11, np12); + if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) { - edgecollpair.p11 = face1->v3; - edgecollpair.p12 = face1->v4; + calculateEENormal(np11, np12, np21, np22, out_normal); + + // return (np22 - (np11 + (np12 - np11) * out_a1)).lengthSquared(); + VECSUB(temp, np12, np11); + VecMulf(temp, *out_a1); + VECADD(temp2, temp, np11); + VECSUB(temp2, np22, temp2); + return INPR(temp2, temp2); } - else + } + else + { // == 0.0 + *out_a1 = projectPointOntoLine(np21, np11, np12); + if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) { - edgecollpair.p11 = face1->v3; - edgecollpair.p12 = face1->v1; - i+=5; // get out of here after this edge pair is handled + calculateEENormal(np11, np11, np21, np22, out_normal); + + // return (np21 - (np11 + (np12 - np11) * out_a1)).lengthSquared(); + VECSUB(temp, np12, np11); + VecMulf(temp, *out_a1); + VECADD(temp2, temp, np11); + VECSUB(temp2, np21, temp2); + return INPR(temp2, temp2); } } - else if ( i == 3 ) + + CLAMP(*out_a1, 0.0, 1.0); + calculateEENormal(np11, np12, np21, np22, out_normal); + if(*out_a1 > .5) { - if ( face1->v4 ) + if(*out_a2 > .5) { - edgecollpair.p11 = face1->v4; - edgecollpair.p12 = face1->v1; + VECSUB(temp, np12, np22); } else - continue; + { + VECSUB(temp, np12, np21); + } } else { - edgecollpair.p11 = face1->v3; - edgecollpair.p12 = face1->v1; - } - - - for ( j = 0; j < 5; j++ ) - { - if ( j == 0 ) - { - edgecollpair.p21 = face2->v1; - edgecollpair.p22 = face2->v2; - } - else if ( j == 1 ) - { - edgecollpair.p21 = face2->v2; - edgecollpair.p22 = face2->v3; - } - else if ( j == 2 ) - { - if ( face2->v4 ) - { - edgecollpair.p21 = face2->v3; - edgecollpair.p22 = face2->v4; - } - else - { - edgecollpair.p21 = face2->v3; - edgecollpair.p22 = face2->v1; - } - } - else if ( j == 3 ) + if(*out_a2 > .5) { - if ( face2->v4 ) - { - edgecollpair.p21 = face2->v4; - edgecollpair.p22 = face2->v1; - } - else - continue; + VECSUB(temp, np11, np22); } else { - edgecollpair.p21 = face2->v3; - edgecollpair.p22 = face2->v1; + VECSUB(temp, np11, np21); } + } + return INPR(temp, temp); + } + else + { + + // If the lines aren't parallel (but coplanar) they have to intersect - if ( !cloth_are_edges_adjacent ( clmd, coll_clmd, &edgecollpair ) ) - { - VECSUB ( a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold ); - VECSUB ( b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v ); - VECSUB ( c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold ); - VECSUB ( d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v ); - VECSUB ( e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold ); - VECSUB ( f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v ); - - numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution ); - - for ( k = 0; k < numsolutions; k++ ) - { - if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) ) - { - //float out_collisionTime = solution[k]; - - // TODO: check for collisions + findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2); - // TODO: put into (edge) collision list + // If both points are on the finite edges, we're done. + if (*out_a1 >= 0.0 && *out_a1 <= 1.0 && *out_a2 >= 0.0 && *out_a2 <= 1.0) + { + float p1[3], p2[3]; + + // p1= np11 + (np12 - np11) * out_a1; + VECSUB(temp, np12, np11); + VecMulf(temp, *out_a1); + VECADD(p1, np11, temp); + + // p2 = np21 + (np22 - np21) * out_a2; + VECSUB(temp, np22, np21); + VecMulf(temp, *out_a2); + VECADD(p2, np21, temp); + + calculateEENormal(np11, np12, np21, np22, out_normal); + VECSUB(temp, p1, p2); + return INPR(temp, temp); + } - // printf("Moving edge found!\n"); - } - } - } + + /* + * Clamp both points to the finite edges. + * The one that moves most during clamping is one part of the solution. + */ + dist_a1 = *out_a1; + CLAMP(dist_a1, 0.0, 1.0); + dist_a2 = *out_a2; + CLAMP(dist_a2, 0.0, 1.0); + + // Now project the "most clamped" point on the other line. + if (dist_a1 > dist_a2) + { + /* keep out_a1 */ + float p1[3]; + + // p1 = np11 + (np12 - np11) * out_a1; + VECSUB(temp, np12, np11); + VecMulf(temp, *out_a1); + VECADD(p1, np11, temp); + + *out_a2 = projectPointOntoLine(p1, np21, np22); + CLAMP(*out_a2, 0.0, 1.0); + + calculateEENormal(np11, np12, np21, np22, out_normal); + + // return (p1 - (np21 + (np22 - np21) * out_a2)).lengthSquared(); + VECSUB(temp, np22, np21); + VecMulf(temp, *out_a2); + VECADD(temp, temp, np21); + VECSUB(temp, p1, temp); + return INPR(temp, temp); + } + else + { + /* keep out_a2 */ + float p2[3]; + + // p2 = np21 + (np22 - np21) * out_a2; + VECSUB(temp, np22, np21); + VecMulf(temp, *out_a2); + VECADD(p2, np21, temp); + + *out_a1 = projectPointOntoLine(p2, np11, np12); + CLAMP(*out_a1, 0.0, 1.0); + + calculateEENormal(np11, np12, np21, np22, out_normal); + + // return ((np11 + (np12 - np11) * out_a1) - p2).lengthSquared(); + VECSUB(temp, np12, np11); + VecMulf(temp, *out_a1); + VECADD(temp, temp, np11); + VECSUB(temp, temp, p2); + return INPR(temp, temp); } } + + printf("Error in edgedge_distance: end of function\n"); + return 0; } -void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 ) +int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair ) { - CollPair collpair; - Cloth *cloth1=NULL, *cloth2=NULL; - MFace *face1=NULL, *face2=NULL; - ClothVertex *verts1=NULL, *verts2=NULL; + EdgeCollPair edgecollpair; + Cloth *cloth1=NULL; + ClothVertex *verts1=NULL; unsigned int i = 0, j = 0, k = 0; int numsolutions = 0; - float a[3], b[3], c[3], d[3], e[3], f[3], solution[3]; + double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3]; + double solution[3], solution2[3]; + MVert *verts2 = collmd->current_x; // old x + MVert *velocity2 = collmd->current_v; // velocity + float distance; + float triA[3][3], triB[3][3]; + int result = 0; + + cloth1 = clmd->clothObject; + verts1 = cloth1->verts; - for ( i = 0; i < 2; i++ ) + for(i = 0; i < 9; i++) { - cloth1 = clmd->clothObject; - cloth2 = coll_clmd->clothObject; + // 9 edge - edge possibilities - verts1 = cloth1->verts; - verts2 = cloth2->verts; + if(i == 0) // cloth edge: 1-2; coll edge: 1-2 + { + edgecollpair.p11 = collpair->ap1; + edgecollpair.p12 = collpair->ap2; - face1 = & ( cloth1->mfaces[tree1->tri_index] ); - face2 = & ( cloth2->mfaces[tree2->tri_index] ); + edgecollpair.p21 = collpair->bp1; + edgecollpair.p22 = collpair->bp2; + } + else if(i == 1) // cloth edge: 1-2; coll edge: 2-3 + { + edgecollpair.p11 = collpair->ap1; + edgecollpair.p12 = collpair->ap2; - // check all possible pairs of triangles - if ( i == 0 ) + edgecollpair.p21 = collpair->bp2; + edgecollpair.p22 = collpair->bp3; + } + else if(i == 2) // cloth edge: 1-2; coll edge: 1-3 { - collpair.ap1 = face1->v1; - collpair.ap2 = face1->v2; - collpair.ap3 = face1->v3; - - collpair.pointsb[0] = face2->v1; - collpair.pointsb[1] = face2->v2; - collpair.pointsb[2] = face2->v3; - collpair.pointsb[3] = face2->v4; + edgecollpair.p11 = collpair->ap1; + edgecollpair.p12 = collpair->ap2; + + edgecollpair.p21 = collpair->bp1; + edgecollpair.p22 = collpair->bp3; } + else if(i == 3) // cloth edge: 2-3; coll edge: 1-2 + { + edgecollpair.p11 = collpair->ap2; + edgecollpair.p12 = collpair->ap3; - if ( i == 1 ) + edgecollpair.p21 = collpair->bp1; + edgecollpair.p22 = collpair->bp2; + } + else if(i == 4) // cloth edge: 2-3; coll edge: 2-3 { - if ( face1->v4 ) - { - collpair.ap1 = face1->v3; - collpair.ap2 = face1->v4; - collpair.ap3 = face1->v1; - - collpair.pointsb[0] = face2->v1; - collpair.pointsb[1] = face2->v2; - collpair.pointsb[2] = face2->v3; - collpair.pointsb[3] = face2->v4; - } - else - i++; + edgecollpair.p11 = collpair->ap2; + edgecollpair.p12 = collpair->ap3; + + edgecollpair.p21 = collpair->bp2; + edgecollpair.p22 = collpair->bp3; } + else if(i == 5) // cloth edge: 2-3; coll edge: 1-3 + { + edgecollpair.p11 = collpair->ap2; + edgecollpair.p12 = collpair->ap3; - // calc SIPcode (?) + edgecollpair.p21 = collpair->bp1; + edgecollpair.p22 = collpair->bp3; + } + else if(i ==6) // cloth edge: 1-3; coll edge: 1-2 + { + edgecollpair.p11 = collpair->ap1; + edgecollpair.p12 = collpair->ap3; - if ( i < 2 ) + edgecollpair.p21 = collpair->bp1; + edgecollpair.p22 = collpair->bp2; + } + else if(i ==7) // cloth edge: 1-3; coll edge: 2-3 { - VECSUB ( a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold ); - VECSUB ( b, verts1[collpair.ap2].v, verts1[collpair.ap1].v ); - VECSUB ( c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold ); - VECSUB ( d, verts1[collpair.ap3].v, verts1[collpair.ap1].v ); + edgecollpair.p11 = collpair->ap1; + edgecollpair.p12 = collpair->ap3; - for ( j = 0; j < 4; j++ ) - { - if ( ( j==3 ) && ! ( face2->v4 ) ) - break; + edgecollpair.p21 = collpair->bp2; + edgecollpair.p22 = collpair->bp3; + } + else if(i == 8) // cloth edge: 1-3; coll edge: 1-3 + { + edgecollpair.p11 = collpair->ap1; + edgecollpair.p12 = collpair->ap3; - VECSUB ( e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold ); - VECSUB ( f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v ); + edgecollpair.p21 = collpair->bp1; + edgecollpair.p22 = collpair->bp3; + } + /* + if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16)) + printf("Ahier!\n"); + if((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3)) + printf("Ahier!\n"); + */ + + // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) ) + { + // 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 ); - numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution ); + VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold ); + VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv ); - for ( k = 0; k < numsolutions; k++ ) + VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold ); + VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv ); + + 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]); + if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) && ( solution[k] > ALMOST_ZERO)) { - if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) ) + float a,b; + float out_normal[3]; + float distance; + float impulse = 0; + float I_mag; + float m1, m2; + + // move verts + 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, 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); + + if ((distance <= clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO) && (INPR(out_normal, out_normal) > 0)) { - //float out_collisionTime = solution[k]; + float vrel_1_to_2[3], temp[3], temp2[3], out_normalVelocity; + float desiredVn; + + VECCOPY(vrel_1_to_2, verts1[edgecollpair.p11].tv); + VecMulf(vrel_1_to_2, 1.0 - a); + VECCOPY(temp, verts1[edgecollpair.p12].tv); + VecMulf(temp, a); - // TODO: check for collisions + VECADD(vrel_1_to_2, vrel_1_to_2, temp); - // TODO: put into (point-face) collision list + VECCOPY(temp, verts1[edgecollpair.p21].tv); + VecMulf(temp, 1.0 - b); + VECCOPY(temp2, verts1[edgecollpair.p22].tv); + VecMulf(temp2, b); + VECADD(temp, temp, temp2); - // printf("Moving found!\n"); + 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); + } +*/ + /* Inelastic repulsion impulse. */ + + // Calculate which normal velocity we need. + desiredVn = (out_normalVelocity * (float)solution[k] - (.1 * (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree )) - sqrt(distance)) - ALMOST_ZERO); + + // Now calculate what impulse we need to reach that velocity. + I_mag = (out_normalVelocity - desiredVn) / 2.0; // / (1/m1 + 1/m2); + + // Finally apply that impulse. + impulse = (2.0 * -I_mag) / (a*a + (1.0-a)*(1.0-a) + b*b + (1.0-b)*(1.0-b)); + + VECADDMUL ( verts1[edgecollpair.p11].impulse, out_normal, (1.0-a) * impulse ); + verts1[edgecollpair.p11].impulse_count++; + + VECADDMUL ( verts1[edgecollpair.p12].impulse, out_normal, a * impulse ); + verts1[edgecollpair.p12].impulse_count++; + + // return true; + result = 1; + break; } - } + else + { + // missing from collision.hpp + } + // mintime = MIN2(mintime, (float)solution[k]); - // TODO: check borders for collisions + break; + } } - } } + return result; } -void cloth_collision_moving ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 ) +int cloth_collision_moving_tris ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair ) { - // TODO: check for adjacent - cloth_collision_moving_edges ( clmd, coll_clmd, tree1, tree2 ); + 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; - cloth_collision_moving_tris ( clmd, coll_clmd, tree1, tree2 ); - cloth_collision_moving_tris ( coll_clmd, clmd, tree2, tree1 ); -} + cloth1 = clmd->clothObject; + verts1 = cloth1->verts; -void cloth_free_collision_list ( ClothModifierData *clmd ) -{ - // free collision list - if ( clmd->coll_parms->collision_list ) + for(i = 0; i < 9; i++) { - LinkNode *search = clmd->coll_parms->collision_list; - while ( search ) + // 9 edge - edge possibilities + + if(i == 0) { - CollPair *coll_pair = search->link; + edgecollpair.p11 = collpair->ap1; + edgecollpair.p12 = collpair->ap2; - MEM_freeN ( coll_pair ); - search = search->next; + edgecollpair.p21 = collpair->bp1; + edgecollpair.p22 = collpair->bp2; } - BLI_linklist_free ( clmd->coll_parms->collision_list,NULL ); + } - clmd->coll_parms->collision_list = NULL; + return result; +} + +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++ ) + { + // only handle moving collisions here + if (!( collpair->flag & COLLISION_IN_FUTURE )) + continue; + + cloth_collision_moving_edges ( clmd, collmd, collpair); + // cloth_collision_moving_tris ( clmd, collmd, collpair); } + + return 1; } int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData *collmd, float step, float dt ) { Cloth *cloth = clmd->clothObject; - BVH *cloth_bvh= ( BVH * ) cloth->tree; + 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; - unsigned int result = 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->bvh ) + if ( collmd->bvhtree ) { /* get pointer to bounding volume hierarchy */ - BVH *coll_bvh = collmd->bvh; + BVHTree *coll_bvh = collmd->bvhtree; /* move object to position (step) in time */ collision_move_object ( collmd, step + dt, step ); /* search for overlapping collision pairs */ - bvh_traverse ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_static, 0 ); + overlap = BLI_bvhtree_overlap ( cloth_bvh, coll_bvh, &result ); + + collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * result*4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision + collisions_index = collisions; + + for ( i = 0; i < result; i++ ) + { + collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, collisions_index ); + } + + if ( overlap ) + MEM_freeN ( overlap ); } else { @@ -994,29 +1414,50 @@ int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData { result = 0; - if ( collmd->bvh ) - result += cloth_collision_response_static ( clmd, collmd ); + if ( collmd->bvhtree ) + { + result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index ); - // apply impulses in parallel - if ( result ) - for ( i = 0; i < numverts; i++ ) + // apply impulses in parallel + if ( result ) { - // calculate "velocities" (just xnew = xold + v; no dt in v) - if ( verts[i].impulse_count ) + for ( i = 0; i < numverts; i++ ) { - VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count ); - VECCOPY ( verts[i].impulse, tnull ); - verts[i].impulse_count = 0; + // 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++; + ret++; + } } } +/* + result += cloth_collision_moving ( clmd, collmd, collisions, collisions_index ); - if ( !result ) - break; + // 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++; + } + } + } +*/ + } } - cloth_free_collision_list ( clmd ); + if ( collisions ) MEM_freeN ( collisions ); return ret; } @@ -1028,22 +1469,22 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt ) CollisionModifierData *collmd=NULL; Cloth *cloth=NULL; Object *coll_ob=NULL; - BVH *cloth_bvh=NULL; - long i=0, j = 0, numfaces = 0, numverts = 0; + 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; ClothVertex *verts = NULL; - int ret = 0; + int ret = 0, ret2 = 0; ClothModifierData *tclmd; int collisions = 0, count = 0; - if ( ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ ) || ! ( ( ( Cloth * ) clmd->clothObject )->tree ) ) + if ( ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ ) || ! ( ( ( Cloth * ) clmd->clothObject )->bvhtree ) ) { return 0; } cloth = clmd->clothObject; verts = cloth->verts; - cloth_bvh = ( BVH * ) cloth->tree; + cloth_bvh = ( BVHTree * ) cloth->bvhtree; numfaces = clmd->clothObject->numfaces; numverts = clmd->clothObject->numverts; @@ -1052,12 +1493,13 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt ) //////////////////////////////////////////////////////////// // update cloth bvh - bvh_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function) + 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) do { result = 0; - clmd->coll_parms->collision_list = NULL; + ret2 = 0; // check all collision objects for ( base = G.scene->base.first; base; base = base->next ) @@ -1086,6 +1528,7 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt ) continue; ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt ); + ret2 += ret; } } } @@ -1096,6 +1539,7 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt ) continue; ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt ); + ret2 += ret; } } rounds++; @@ -1126,97 +1570,121 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt ) //////////////////////////////////////////////////////////// if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF ) { + + MFace *mface = cloth->mfaces; + BVHTreeOverlap *overlap = NULL; + collisions = 1; verts = cloth->verts; // needed for openMP - for ( count = 0; count < clmd->coll_parms->self_loop_count; count++ ) + numfaces = clmd->clothObject->numfaces; + numverts = clmd->clothObject->numverts; + + verts = cloth->verts; + + if ( cloth->bvhselftree ) { - if ( collisions ) + // 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++ ) { - collisions = 0; -#pragma omp parallel for private(i,j, collisions) shared(verts, ret) - for ( i = 0; i < cloth->numverts; i++ ) + 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 ) { - for ( j = i + 1; j < cloth->numverts; j++ ) + 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 = 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 ) - { - 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, i, j ) ) - { - continue; - } - - length = Normalize ( temp ); - - if ( length < mindistance ) - { - 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 ); - } - - collisions = 1; - - if ( !ret ) - { -#pragma omp critical - { - ret = 1; - } - } - } + 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 ) + { + 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 + { + // check for approximated time collisions + } } + + if ( overlap ) + MEM_freeN ( overlap ); + } //////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////// // SELFCOLLISIONS: update velocities //////////////////////////////////////////////////////////// - if ( ret ) + if ( ret2 ) { for ( i = 0; i < cloth->numverts; i++ ) { - if ( ! ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) + if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) + { VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold ); + } } } //////////////////////////////////////////////////////////// } } - while ( result && ( clmd->coll_parms->loop_count>rounds ) ); + while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) ); 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 diff --git a/source/blender/blenkernel/intern/kdop.c b/source/blender/blenkernel/intern/kdop.c deleted file mode 100644 index 3189fe960ad..00000000000 --- a/source/blender/blenkernel/intern/kdop.c +++ /dev/null @@ -1,860 +0,0 @@ -/* kdop.c -* -* -* ***** BEGIN GPL LICENSE BLOCK ***** -* -* This program is free software; you can redistribute it and/or -* modify it under the terms of the GNU General Public License -* as published by the Free Software Foundation; either version 2 -* of the License, or (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software Foundation, -* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. -* -* The Original Code is Copyright (C) Blender Foundation -* All rights reserved. -* -* The Original Code is: all of this file. -* -* Contributor(s): none yet. -* -* ***** END GPL LICENSE BLOCK ***** -*/ - -#include "MEM_guardedalloc.h" - -#include "BKE_cloth.h" - -#include "DNA_cloth_types.h" -#include "DNA_mesh_types.h" -#include "DNA_scene_types.h" - -#include "BKE_deform.h" -#include "BKE_DerivedMesh.h" -#include "BKE_cdderivedmesh.h" -#include "BKE_effect.h" -#include "BKE_global.h" -#include "BKE_object.h" -#include "BKE_modifier.h" -#include "BKE_utildefines.h" - -#ifdef _OPENMP -#include <omp.h> -#endif - - -//////////////////////////////////////////////////////////////////////// -// Additional fastened appending function -// It uses the link to the last inserted node as start value -// for searching the end of the list -// NEW: in compare to the original function, this one returns -// the reference to the last inserted node -//////////////////////////////////////////////////////////////////////// -LinkNode *BLI_linklist_append_fast(LinkNode **listp, void *ptr) { - LinkNode *nlink= MEM_mallocN(sizeof(*nlink), "nlink"); - LinkNode *node = *listp; - - nlink->link = ptr; - nlink->next = NULL; - - if(node == NULL){ - *listp = nlink; - } else { - while(node->next != NULL){ - node = node->next; - } - node->next = nlink; - } - return nlink; -} - - - -//////////////////////////////////////////////////////////////////////// -// Bounding Volume Hierarchy Definition -// -// Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below -// Notes: You have to choose the type at compile time ITM -// Notes: You can choose the tree type --> binary, quad, octree, choose below -//////////////////////////////////////////////////////////////////////// - -static float KDOP_AXES[13][3] = -{ {1.0, 0, 0}, {0, 1.0, 0}, {0, 0, 1.0}, {1.0, 1.0, 1.0}, {1.0, -1.0, 1.0}, {1.0, 1.0, -1.0}, -{1.0, -1.0, -1.0}, {1.0, 1.0, 0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}, {1.0, -1.0, 0}, {1.0, 0, -1.0}, -{0, 1.0, -1.0} -}; - -///////////// choose bounding volume here! ///////////// - -#define KDOP_26 - - - -#ifdef KDOP_26 -#define KDOP_END 13 -#define KDOP_START 0 -#endif - -#ifdef KDOP_18 -#define KDOP_END 13 -#define KDOP_START 7 -#endif - -#ifdef KDOP_14 -#define KDOP_END 7 -#define KDOP_START 0 -#endif - -// this is basicly some AABB -#ifdef KDOP_8 -#define KDOP_END 4 -#define KDOP_START 0 -#endif - -// this is basicly some OBB -#ifdef KDOP_6 -#define KDOP_END 3 -#define KDOP_START 0 -#endif - -////////////////////////////////////////////////////////////////////////////////////////////////////// -// Introsort -// with permission deriven from the following Java code: -// http://ralphunden.net/content/tutorials/a-guide-to-introsort/ -// and he derived it from the SUN STL -////////////////////////////////////////////////////////////////////////////////////////////////////// -static int size_threshold = 16; -/* -* Common methods for all algorithms -*/ -DO_INLINE void bvh_exchange(CollisionTree **a, int i, int j) -{ - CollisionTree *t=a[i]; - a[i]=a[j]; - a[j]=t; -} -DO_INLINE int floor_lg(int a) -{ - return (int)(floor(log(a)/log(2))); -} - -/* -* Insertion sort algorithm -*/ -void bvh_insertionsort(CollisionTree **a, int lo, int hi, int axis) -{ - int i,j; - CollisionTree *t; - for (i=lo; i < hi; i++) - { - j=i; - t = a[i]; - while((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis])) - { - a[j] = a[j-1]; - j--; - } - a[j] = t; - } -} - -static int bvh_partition(CollisionTree **a, int lo, int hi, CollisionTree * x, int axis) -{ - int i=lo, j=hi; - while (1) - { - while ((a[i])->bv[axis] < x->bv[axis]) i++; - j=j-1; - while (x->bv[axis] < (a[j])->bv[axis]) j=j-1; - if(!(i < j)) - return i; - bvh_exchange(a, i,j); - i++; - } -} - -/* -* Heapsort algorithm -*/ -static void bvh_downheap(CollisionTree **a, int i, int n, int lo, int axis) -{ - CollisionTree * d = a[lo+i-1]; - int child; - while (i<=n/2) - { - child = 2*i; - if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis])) - { - child++; - } - if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break; - a[lo+i-1] = a[lo+child-1]; - i = child; - } - a[lo+i-1] = d; -} - -static void bvh_heapsort(CollisionTree **a, int lo, int hi, int axis) -{ - int n = hi-lo, i; - for (i=n/2; i>=1; i=i-1) - { - bvh_downheap(a, i,n,lo, axis); - } - for (i=n; i>1; i=i-1) - { - bvh_exchange(a, lo,lo+i-1); - bvh_downheap(a, 1,i-1,lo, axis); - } -} - -static CollisionTree *bvh_medianof3(CollisionTree **a, int lo, int mid, int hi, int axis) // returns Sortable -{ - if ((a[mid])->bv[axis] < (a[lo])->bv[axis]) - { - if ((a[hi])->bv[axis] < (a[mid])->bv[axis]) - return a[mid]; - else - { - if ((a[hi])->bv[axis] < (a[lo])->bv[axis]) - return a[hi]; - else - return a[lo]; - } - } - else - { - if ((a[hi])->bv[axis] < (a[mid])->bv[axis]) - { - if ((a[hi])->bv[axis] < (a[lo])->bv[axis]) - return a[lo]; - else - return a[hi]; - } - else - return a[mid]; - } -} -/* -* Quicksort algorithm modified for Introsort -*/ -void bvh_introsort_loop (CollisionTree **a, int lo, int hi, int depth_limit, int axis) -{ - int p; - - while (hi-lo > size_threshold) - { - if (depth_limit == 0) - { - bvh_heapsort(a, lo, hi, axis); - return; - } - depth_limit=depth_limit-1; - p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis); - bvh_introsort_loop(a, p, hi, depth_limit, axis); - hi=p; - } -} - -DO_INLINE void bvh_sort(CollisionTree **a0, int begin, int end, int axis) -{ - if (begin < end) - { - CollisionTree **a=a0; - bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis); - bvh_insertionsort(a, begin, end, axis); - } -} -DO_INLINE void bvh_sort_along_axis(CollisionTree **face_list, int start, int end, int axis) -{ - bvh_sort(face_list, start, end, axis); -} -//////////////////////////////////////////////////////////////////////////////////////////////// -void bvh_free(BVH * bvh) -{ - LinkNode *search = NULL; - CollisionTree *tree = NULL; - - if (bvh) - { - - search = bvh->tree; - - while(search) - { - LinkNode *next= search->next; - tree = search->link; - - MEM_freeN(tree); - - search = next; - } - - BLI_linklist_free(bvh->tree,NULL); - bvh->tree = NULL; - - if(bvh->current_x) - MEM_freeN(bvh->current_x); - if(bvh->current_xold) - MEM_freeN(bvh->current_xold); - - MEM_freeN(bvh); - bvh = NULL; - } -} - -// only supports x,y,z axis in the moment -// but we should use a plain and simple function here for speed sake -DO_INLINE int bvh_largest_axis(float *bv) -{ - float middle_point[3]; - - middle_point[0] = (bv[1]) - (bv[0]); // x axis - middle_point[1] = (bv[3]) - (bv[2]); // y axis - middle_point[2] = (bv[5]) - (bv[4]); // z axis - if (middle_point[0] > middle_point[1]) - { - if (middle_point[0] > middle_point[2]) - return 1; // max x axis - else - return 5; // max z axis - } - else - { - if (middle_point[1] > middle_point[2]) - return 3; // max y axis - else - return 5; // max z axis - } -} - -// depends on the fact that the BVH's for each face is already build -DO_INLINE void bvh_calc_DOP_hull_from_faces(BVH * bvh, CollisionTree **tri, int numfaces, float *bv) -{ - float newmin,newmax; - int i, j; - - if(numfaces >0) - { - // for all Axes. - for (i = KDOP_START; i < KDOP_END; i++) - { - bv[(2 * i)] = (tri [0])->bv[(2 * i)]; - bv[(2 * i) + 1] = (tri [0])->bv[(2 * i) + 1]; - } - } - - for (j = 0; j < numfaces; j++) - { - // for all Axes. - for (i = KDOP_START; i < KDOP_END; i++) - { - newmin = (tri [j])->bv[(2 * i)]; - if ((newmin < bv[(2 * i)])) - { - bv[(2 * i)] = newmin; - } - - newmax = (tri [j])->bv[(2 * i) + 1]; - if ((newmax > bv[(2 * i) + 1])) - { - bv[(2 * i) + 1] = newmax; - } - } - } -} - -DO_INLINE void bvh_calc_DOP_hull_static(BVH * bvh, CollisionTree **tri, int numfaces, float *bv, CollisionTree *tree) -{ - MFace *tempMFace = bvh->mfaces; - float *tempBV = bv; - float newminmax; - int i, j, k; - - for (j = 0; j < numfaces; j++) - { - tempMFace = bvh->mfaces + (tri [j])->tri_index; - // 3 or 4 vertices per face. - for (k = 0; k < 4; k++) - { - int temp = 0; - // If this is a triangle. - if (k == 3 && !tempMFace->v4) - continue; - // TODO: other name for "temp" this gets all vertices of a face - if (k == 0) - temp = tempMFace->v1; - else if (k == 1) - temp = tempMFace->v2; - else if (k == 2) - temp = tempMFace->v3; - else if (k == 3) - temp = tempMFace->v4; - // for all Axes. - for (i = KDOP_START; i < KDOP_END; i++) - { - newminmax = INPR(bvh->current_xold[temp].co, KDOP_AXES[i]); - if ((newminmax < tempBV[(2 * i)]) || (k == 0 && j == 0)) - tempBV[(2 * i)] = newminmax; - if ((newminmax > tempBV[(2 * i) + 1])|| (k == 0 && j == 0)) - tempBV[(2 * i) + 1] = newminmax; - } - } - - /* calculate normal of this face */ - /* (code copied from cdderivedmesh.c) */ - /* - if(tempMFace->v4) - CalcNormFloat4(bvh->current_xold[tempMFace->v1].co, bvh->current_xold[tempMFace->v2].co, - bvh->current_xold[tempMFace->v3].co, bvh->current_xold[tempMFace->v4].co, tree->normal); - else - CalcNormFloat(bvh->current_xold[tempMFace->v1].co, bvh->current_xold[tempMFace->v2].co, - bvh->current_xold[tempMFace->v3].co, tree->normal); - - tree->alpha = 0; - */ - } -} - -DO_INLINE void bvh_calc_DOP_hull_moving(BVH * bvh, CollisionTree **tri, int numfaces, float *bv, CollisionTree *tree) -{ - MFace *tempMFace = bvh->mfaces; - float *tempBV = bv; - float newminmax; - int i, j, k; - - /* TODO: calculate normals */ - - for (j = 0; j < numfaces; j++) - { - tempMFace = bvh->mfaces + (tri [j])->tri_index; - // 3 or 4 vertices per face. - for (k = 0; k < 4; k++) - { - int temp = 0; - // If this is a triangle. - if (k == 3 && !tempMFace->v4) - continue; - // TODO: other name for "temp" this gets all vertices of a face - if (k == 0) - temp = tempMFace->v1; - else if (k == 1) - temp = tempMFace->v2; - else if (k == 2) - temp = tempMFace->v3; - else if (k == 3) - temp = tempMFace->v4; - // for all Axes. - for (i = KDOP_START; i < KDOP_END; i++) - { - newminmax = INPR(bvh->current_xold[temp].co, KDOP_AXES[i]); - if ((newminmax < tempBV[(2 * i)]) || (k == 0 && j == 0)) - tempBV[(2 * i)] = newminmax; - if ((newminmax > tempBV[(2 * i) + 1])|| (k == 0 && j == 0)) - tempBV[(2 * i) + 1] = newminmax; - - newminmax = INPR(bvh->current_x[temp].co, KDOP_AXES[i]); - if ((newminmax < tempBV[(2 * i)]) || (k == 0 && j == 0)) - tempBV[(2 * i)] = newminmax; - if ((newminmax > tempBV[(2 * i) + 1])|| (k == 0 && j == 0)) - tempBV[(2 * i) + 1] = newminmax; - } - } - } -} - -static void bvh_div_env_node(BVH *bvh, CollisionTree *tree, CollisionTree **face_list, unsigned int start, unsigned int end, int lastaxis, LinkNode *nlink) -{ - int i = 0; - CollisionTree *newtree = NULL; - int laxis = 0, max_nodes=4; - unsigned int tstart, tend; - LinkNode *nlink1 = nlink; - LinkNode *tnlink; - tree->traversed = 0; - // Determine which axis to split along - laxis = bvh_largest_axis(tree->bv); - - // Sort along longest axis - if(laxis!=lastaxis) - bvh_sort_along_axis(face_list, start, end, laxis); - - // maximum is 4 since we have a quad tree - max_nodes = MIN2((end-start + 1 ),4); - - for (i = 0; i < max_nodes; i++) - { - tree->count_nodes++; - - if(end-start+1 > 4) - { - int quarter = ((float)((float)(end - start + 1) / 4.0f)); - tstart = start + i * quarter; - tend = tstart + quarter - 1; - - // be sure that we get all faces - if(i==3) - { - tend = end; - } - } - else - { - tend = tstart = start + i; - } - - // Build tree until 4 node left. - if ((tend-tstart + 1 ) > 1) - { - newtree = (CollisionTree *)MEM_callocN(sizeof(CollisionTree), "CollisionTree"); - tnlink = BLI_linklist_append_fast(&nlink1->next, newtree); - - newtree->nodes[0] = newtree->nodes[1] = newtree->nodes[2] = newtree->nodes[3] = NULL; - newtree->count_nodes = 0; - newtree->parent = tree; - newtree->isleaf = 0; - - tree->nodes[i] = newtree; - - nlink1 = tnlink; - - bvh_calc_DOP_hull_from_faces(bvh, &face_list[tstart], tend-tstart + 1, tree->nodes[i]->bv); - - bvh_div_env_node(bvh, tree->nodes[i], face_list, tstart, tend, laxis, nlink1); - } - else // ok, we have 1 left for this node - { - CollisionTree *tnode = face_list[tstart]; - tree->nodes[i] = tnode; - tree->nodes[i]->parent = tree; - } - } - return; -} - -/* function cannot be directly called - needs alloced bvh */ -void bvh_build (BVH *bvh) -{ - unsigned int i = 0, j = 0, k = 0; - CollisionTree **face_list=NULL; - CollisionTree *tree=NULL; - LinkNode *nlink = NULL; - - bvh->flags = 0; - bvh->leaf_tree = NULL; - bvh->leaf_root = NULL; - bvh->tree = NULL; - - if(!bvh->current_x) - { - bvh_free(bvh); - return; - } - - bvh->current_xold = MEM_dupallocN(bvh->current_x); - - tree = (CollisionTree *)MEM_callocN(sizeof(CollisionTree), "CollisionTree"); - - if (tree == NULL) - { - printf("bvh_build: Out of memory for nodes.\n"); - bvh_free(bvh); - return; - } - - BLI_linklist_append(&bvh->tree, tree); - - nlink = bvh->tree; - - bvh->root = bvh->tree->link; - bvh->root->isleaf = 0; - bvh->root->parent = NULL; - bvh->root->nodes[0] = bvh->root->nodes[1] = bvh->root->nodes[1] = bvh->root->nodes[3] = NULL; - - if(bvh->numfaces<=1) - { - bvh->root->tri_index = 0; // Why that? --> only one face there - bvh->root->isleaf = 1; - bvh->root->traversed = 0; - bvh->root->count_nodes = 0; - bvh->leaf_root = bvh->root; - bvh->leaf_tree = bvh->root; - bvh->root->nextLeaf = NULL; - bvh->root->prevLeaf = NULL; - } - else - { - // create face boxes - face_list = MEM_callocN (bvh->numfaces * sizeof (CollisionTree *), "CollisionTree"); - if (face_list == NULL) - { - printf("bvh_build: Out of memory for face_list.\n"); - bvh_free(bvh); - return; - } - - // create face boxes - for(i = 0, k = 0; i < bvh->numfaces; i++) - { - LinkNode *tnlink; - - tree = (CollisionTree *)MEM_callocN(sizeof(CollisionTree), "CollisionTree"); - // TODO: check succesfull alloc - - tnlink = BLI_linklist_append_fast(&nlink->next, tree); - - face_list[i] = tree; - tree->tri_index = i; - tree->isleaf = 1; - tree->nextLeaf = NULL; - tree->prevLeaf = bvh->leaf_tree; - tree->parent = NULL; - tree->count_nodes = 0; - - if(i==0) - { - bvh->leaf_tree = bvh->leaf_root = tree; - } - else - { - bvh->leaf_tree->nextLeaf = tree; - bvh->leaf_tree = bvh->leaf_tree->nextLeaf; - } - - tree->nodes[0] = tree->nodes[1] = tree->nodes[2] = tree->nodes[3] = NULL; - - bvh_calc_DOP_hull_static(bvh, &face_list[i], 1, tree->bv, tree); - - // inflate the bv with some epsilon - for (j = KDOP_START; j < KDOP_END; j++) - { - tree->bv[(2 * j)] -= bvh->epsilon; // minimum - tree->bv[(2 * j) + 1] += bvh->epsilon; // maximum - } - - nlink = tnlink; - } - - // build root bvh - bvh_calc_DOP_hull_from_faces(bvh, face_list, bvh->numfaces, bvh->root->bv); - - // This is the traversal function. - bvh_div_env_node(bvh, bvh->root, face_list, 0, bvh->numfaces-1, 0, nlink); - if (face_list) - MEM_freeN(face_list); - - } - -} - -// bvh_overlap - is it possbile for 2 bv's to collide ? -DO_INLINE int bvh_overlap(float *bv1, float *bv2) -{ - int i = 0; - for (i = KDOP_START; i < KDOP_END; i++) - { - // Minimum test. - if (bv1[(2 * i)] > bv2[(2 * i) + 1]) - { - return 0; - } - // Maxiumum test. - if (bv2[(2 * i)] > bv1[(2 * i) + 1]) - { - return 0; - } - } - - return 1; -} - -// bvh_overlap_self - is it possbile for 2 bv's to selfcollide ? -DO_INLINE int bvh_overlap_self(CollisionTree * tree1, CollisionTree * tree2) -{ - // printf("overlap: %f, q: %f\n", (saacos(INPR(tree1->normal, tree2->normal)) / 2.0)+MAX2(tree1->alpha, tree2->alpha), saacos(INPR(tree1->normal, tree2->normal))); - - if((saacos(INPR(tree1->normal, tree2->normal)) / 2.0)+MAX2(tree1->alpha, tree2->alpha) > M_PI) - { - return 1; - } - else - return 0; -} - -/** - * bvh_traverse - traverse two bvh trees looking for potential collisions. - * - * max collisions are n*n collisions --> every triangle collide with - * every other triangle that doesn't require any realloc, but uses - * much memory - */ -int bvh_traverse ( ModifierData * md1, ModifierData * md2, CollisionTree * tree1, CollisionTree * tree2, float step, CM_COLLISION_RESPONSE collision_response, int selfcollision) -{ - int i = 0, ret=0, overlap = 0; - - /* - // Shouldn't be possible - if(!tree1 || !tree2) - { - printf("Error: no tree there\n"); - return 0; -} - */ - - if(selfcollision) - overlap = bvh_overlap_self(tree1, tree2); - else - overlap = bvh_overlap(tree1->bv, tree2->bv); - - if (overlap) - { - // Check if this node in the first tree is a leaf - if (tree1->isleaf) - { - // Check if this node in the second tree a leaf - if (tree2->isleaf) - { - // Provide the collision response. - - if(collision_response) - collision_response (md1, md2, tree1, tree2); - return 1; - } - else - { - // Process the quad tree. - for (i = 0; i < 4; i++) - { - // Only traverse nodes that exist. - if (tree2->nodes[i] && bvh_traverse (md1, md2, tree1, tree2->nodes[i], step, collision_response, selfcollision)) - ret = 1; - } - } - } - else - { - // Process the quad tree. - for (i = 0; i < 4; i++) - { - // Only traverse nodes that exist. - if (tree1->nodes [i] && bvh_traverse (md1, md2, tree1->nodes[i], tree2, step, collision_response, selfcollision)) - ret = 1; - } - } - } - - return ret; -} -// bottom up update of bvh tree: -// join the 4 children here -void bvh_join(CollisionTree *tree) -{ - int i = 0, j = 0; - float max = 0; - - if (!tree) - return; - - for (i = 0; i < 4; i++) - { - if (tree->nodes[i]) - { - for (j = KDOP_START; j < KDOP_END; j++) - { - // update minimum - if ((tree->nodes[i]->bv[(2 * j)] < tree->bv[(2 * j)]) || (i == 0)) - { - tree->bv[(2 * j)] = tree->nodes[i]->bv[(2 * j)]; - } - // update maximum - if ((tree->nodes[i]->bv[(2 * j) + 1] > tree->bv[(2 * j) + 1])|| (i == 0)) - { - tree->bv[(2 * j) + 1] = tree->nodes[i]->bv[(2 * j) + 1]; - } - } - - /* for selfcollisions */ - /* - if(!i) - { - tree->alpha = tree->nodes[i]->alpha; - VECCOPY(tree->normal, tree->nodes[i]->normal); - } - else - { - tree->alpha += saacos(INPR(tree->normal, tree->nodes[i]->normal)) / 2.0; - VECADD(tree->normal, tree->normal, tree->nodes[i]->normal); - VecMulf(tree->normal, 0.5); - max = MAX2(max, tree->nodes[i]->alpha); - } - */ - - } - else - break; - } - - tree->alpha += max; -} - -// update static bvh -/* you have to update the bvh position before calling this function */ -void bvh_update(BVH * bvh, int moving) -{ - CollisionTree *leaf, *parent; - int traversecheck = 1; // if this is zero we don't go further - unsigned int j = 0; - - for (leaf = bvh->leaf_root; leaf; leaf = leaf->nextLeaf) - { - traversecheck = 1; - if ((leaf->parent) && (leaf->parent->traversed == leaf->parent->count_nodes)) - { - leaf->parent->traversed = 0; - } - if(!moving) - bvh_calc_DOP_hull_static(bvh, &leaf, 1, leaf->bv, leaf); - else - bvh_calc_DOP_hull_moving(bvh, &leaf, 1, leaf->bv, leaf); - - // inflate the bv with some epsilon - for (j = KDOP_START; j < KDOP_END; j++) - { - leaf->bv[(2 * j)] -= bvh->epsilon; // minimum - leaf->bv[(2 * j) + 1] += bvh->epsilon; // maximum - } - - for (parent = leaf->parent; parent; parent = parent->parent) - { - if (traversecheck) - { - parent->traversed++; // we tried to go up in hierarchy - if (parent->traversed < parent->count_nodes) - { - traversecheck = 0; - - if (parent->parent) - { - if (parent->parent->traversed == parent->parent->count_nodes) - { - parent->parent->traversed = 0; - } - } - break; // we do not need to check further - } - else - { - bvh_join(parent); - } - } - - } - } -} - diff --git a/source/blender/blenkernel/intern/modifier.c b/source/blender/blenkernel/intern/modifier.c index efc250fdc0d..18912e32e3c 100644 --- a/source/blender/blenkernel/intern/modifier.c +++ b/source/blender/blenkernel/intern/modifier.c @@ -43,6 +43,7 @@ #include "BLI_arithb.h" #include "BLI_blenlib.h" +#include "BLI_kdopbvh.h" #include "BLI_kdtree.h" #include "BLI_linklist.h" #include "BLI_rand.h" @@ -5193,7 +5194,7 @@ static void collisionModifier_initData(ModifierData *md) collmd->current_v = NULL; collmd->time = -1; collmd->numverts = 0; - collmd->bvh = NULL; + collmd->bvhtree = NULL; } static void collisionModifier_freeData(ModifierData *md) @@ -5202,8 +5203,8 @@ static void collisionModifier_freeData(ModifierData *md) if (collmd) { - if(collmd->bvh) - bvh_free(collmd->bvh); + if(collmd->bvhtree) + BLI_bvhtree_free(collmd->bvhtree); if(collmd->x) MEM_freeN(collmd->x); if(collmd->xnew) @@ -5214,7 +5215,6 @@ static void collisionModifier_freeData(ModifierData *md) MEM_freeN(collmd->current_xnew); if(collmd->current_v) MEM_freeN(collmd->current_v); - if(collmd->mfaces) MEM_freeN(collmd->mfaces); @@ -5225,7 +5225,7 @@ static void collisionModifier_freeData(ModifierData *md) collmd->current_v = NULL; collmd->time = -1; collmd->numverts = 0; - collmd->bvh = NULL; + collmd->bvhtree = NULL; collmd->mfaces = NULL; } } @@ -5293,9 +5293,8 @@ static void collisionModifier_deformVerts( collmd->mfaces = dm->dupFaceArray(dm); collmd->numfaces = dm->getNumFaces(dm); - // TODO: epsilon // create bounding box hierarchy - collmd->bvh = bvh_build_from_mvert(collmd->mfaces, collmd->numfaces, collmd->x, numverts, ob->pd->pdef_sboft); + collmd->bvhtree = bvhtree_build_from_mvert(collmd->mfaces, collmd->numfaces, collmd->x, numverts, ob->pd->pdef_sboft); collmd->time = current_time; } @@ -5318,25 +5317,25 @@ static void collisionModifier_deformVerts( memcpy(collmd->current_x, collmd->x, numverts*sizeof(MVert)); /* check if GUI setting has changed for bvh */ - if(collmd->bvh) + if(collmd->bvhtree) { - if(ob->pd->pdef_sboft != collmd->bvh->epsilon) + if(ob->pd->pdef_sboft != BLI_bvhtree_getepsilon(collmd->bvhtree)) { - bvh_free(collmd->bvh); - collmd->bvh = bvh_build_from_mvert(collmd->mfaces, collmd->numfaces, collmd->current_x, numverts, ob->pd->pdef_sboft); + BLI_bvhtree_free(collmd->bvhtree); + collmd->bvhtree = bvhtree_build_from_mvert(collmd->mfaces, collmd->numfaces, collmd->current_x, numverts, ob->pd->pdef_sboft); } } - /* happens on file load (ONLY when i decomment changes in readfile.c */ - if(!collmd->bvh) + /* happens on file load (ONLY when i decomment changes in readfile.c) */ + if(!collmd->bvhtree) { - collmd->bvh = bvh_build_from_mvert(collmd->mfaces, collmd->numfaces, collmd->current_x, numverts, ob->pd->pdef_sboft); + collmd->bvhtree = bvhtree_build_from_mvert(collmd->mfaces, collmd->numfaces, collmd->current_x, numverts, ob->pd->pdef_sboft); } else { // recalc static bounding boxes - bvh_update_from_mvert(collmd->bvh, collmd->current_x, numverts, NULL, 0); + bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 ); } collmd->time = current_time; |