diff options
| -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 | ||||
| -rw-r--r-- | source/blender/blenlib/BLI_kdopbvh.h | 60 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/BLI_kdopbvh.c | 811 | ||||
| -rw-r--r-- | source/blender/blenloader/intern/readfile.c | 2 | ||||
| -rw-r--r-- | source/blender/makesdna/DNA_modifier_types.h | 2 |
10 files changed, 2060 insertions, 1540 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; diff --git a/source/blender/blenlib/BLI_kdopbvh.h b/source/blender/blenlib/BLI_kdopbvh.h new file mode 100644 index 00000000000..b81ff0ee66f --- /dev/null +++ b/source/blender/blenlib/BLI_kdopbvh.h @@ -0,0 +1,60 @@ +/** + * + * ***** 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) 2006 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): Daniel Genrich, Andre Pinto + * + * ***** END GPL LICENSE BLOCK ***** + */ + + +#ifndef BLI_KDOPBVH_H +#define BLI_KDOPBVH_H + +#include <float.h> + +struct BVHTree; +typedef struct BVHTree BVHTree; + +typedef struct BVHTreeOverlap { + int indexA; + int indexB; +} BVHTreeOverlap; + +BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis); +void BLI_bvhtree_free(BVHTree *tree); + +/* construct: first insert points, then call balance */ +int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints); +void BLI_bvhtree_balance(BVHTree *tree); + +/* update: first update points/nodes, then call update_tree to refit the bounding volumes */ +int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints); +void BLI_bvhtree_update_tree(BVHTree *tree); + +/* collision/overlap: check two trees if they overlap, alloc's *overlap with length of the int return value */ +BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result); + +float BLI_bvhtree_getepsilon(BVHTree *tree); + +#endif // BLI_KDOPBVH_H + diff --git a/source/blender/blenlib/intern/BLI_kdopbvh.c b/source/blender/blenlib/intern/BLI_kdopbvh.c new file mode 100644 index 00000000000..9c4238431dc --- /dev/null +++ b/source/blender/blenlib/intern/BLI_kdopbvh.c @@ -0,0 +1,811 @@ +/** + * + * ***** 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) 2006 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): Daniel Genrich, Andre Pinto + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#include "math.h" +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#include "MEM_guardedalloc.h" + +#include "BKE_utildefines.h" + +#include "BLI_kdopbvh.h" +#include "BLI_arithb.h" + +#ifdef _OPENMP +#include <omp.h> +#endif + +typedef struct BVHNode +{ + struct BVHNode **children; // max 8 children + struct BVHNode *parent; // needed for bottom - top update + float *bv; // Bounding volume of all nodes, max 13 axis + int index; /* face, edge, vertex index */ + char totnode; // how many nodes are used, used for speedup + char traversed; // how many nodes already traversed until this level? + char main_axis; +} BVHNode; + +struct BVHTree +{ + BVHNode **nodes; + BVHNode *nodearray; /* pre-alloc branch nodes */ + BVHNode **nodechild; // pre-alloc childs for nodes + float *nodebv; // pre-alloc bounding-volumes for nodes + float epsilon; /* epslion is used for inflation of the k-dop */ + int totleaf; // leafs + int totbranch; + char tree_type; // type of tree (4 => quadtree) + char axis; // kdop type (6 => OBB, 7 => AABB, ...) + char start_axis, stop_axis; // KDOP_AXES array indices according to axis +}; + +typedef struct BVHOverlapData +{ + BVHTree *tree1, *tree2; + BVHTreeOverlap *overlap; + int i, max_overlap; /* i is number of overlaps */ +} BVHOverlapData; +//////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////// +// 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} +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////// +// 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 +*/ +static int floor_lg(int a) +{ + return (int)(floor(log(a)/log(2))); +} + +/* +* Insertion sort algorithm +*/ +static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis) +{ + int i,j; + BVHNode *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(BVHNode **a, int lo, int hi, BVHNode * x, int axis) +{ + int i=lo, j=hi; + while (1) + { + while ((a[i])->bv[axis] < x->bv[axis]) i++; + j--; + while (x->bv[axis] < (a[j])->bv[axis]) j--; + if(!(i < j)) + return i; + SWAP( BVHNode* , a[i], a[j]); + i++; + } +} + +/* +* Heapsort algorithm +*/ +static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis) +{ + BVHNode * 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(BVHNode **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) + { + SWAP(BVHNode*, a[lo],a[lo+i-1]); + bvh_downheap(a, 1,i-1,lo, axis); + } +} + +static BVHNode *bvh_medianof3(BVHNode **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 +*/ +static void bvh_introsort_loop (BVHNode **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; + } +} + +static void sort(BVHNode **a0, int begin, int end, int axis) +{ + if (begin < end) + { + BVHNode **a=a0; + bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis); + bvh_insertionsort(a, begin, end, axis); + } +} +void sort_along_axis(BVHTree *tree, int start, int end, int axis) +{ + sort(tree->nodes, start, end, axis); +} + +//after a call to this function you can expect one of: +// every node to left of a[n] are smaller or equal to it +// every node to the right of a[n] are greater or equal to it +int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){ + int begin = _begin, end = _end, cut; + int i; + while(end-begin > 3) + { + cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis ); + if(cut <= n) + begin = cut; + else + end = cut; + } + bvh_insertionsort(a, begin, end, axis); + + return n; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////// + +void BLI_bvhtree_free(BVHTree *tree) +{ + if(tree) + { + MEM_freeN(tree->nodes); + MEM_freeN(tree->nodearray); + MEM_freeN(tree->nodebv); + MEM_freeN(tree->nodechild); + MEM_freeN(tree); + } +} + +BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis) +{ + BVHTree *tree; + int numbranches=0, i; + + // only support up to octree + if(tree_type > 8) + return NULL; + + tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree"); + + if(tree) + { + tree->epsilon = epsilon; + tree->tree_type = tree_type; + tree->axis = axis; + + if(axis == 26) + { + tree->start_axis = 0; + tree->stop_axis = 13; + } + else if(axis == 18) + { + tree->start_axis = 7; + tree->stop_axis = 13; + } + else if(axis == 14) + { + tree->start_axis = 0; + tree->stop_axis = 7; + } + else if(axis == 8) // AABB + { + tree->start_axis = 0; + tree->stop_axis = 4; + } + else if(axis == 6) // OBB + { + tree->start_axis = 0; + tree->stop_axis = 3; + } + else + { + MEM_freeN(tree); + return NULL; + } + + + // calculate max number of branches, our bvh kdop is "almost perfect" + for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++) + numbranches += (pow(tree_type, i) / tree_type); + + tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes"); + + if(!tree->nodes) + { + MEM_freeN(tree); + return NULL; + } + + tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * (numbranches+maxsize + tree_type), "BVHNodeBV"); + if(!tree->nodebv) + { + MEM_freeN(tree->nodes); + MEM_freeN(tree); + } + + tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * (numbranches+maxsize + tree_type), "BVHNodeBV"); + if(!tree->nodechild) + { + MEM_freeN(tree->nodebv); + MEM_freeN(tree->nodes); + MEM_freeN(tree); + } + + tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray"); + + if(!tree->nodearray) + { + MEM_freeN(tree->nodechild); + MEM_freeN(tree->nodebv); + MEM_freeN(tree->nodes); + MEM_freeN(tree); + return NULL; + } + + //link the dynamic bv and child links + for(i=0; i< numbranches+maxsize + tree_type; i++) + { + tree->nodearray[i].bv = tree->nodebv + i * axis; + tree->nodearray[i].children = tree->nodechild + i * tree_type; + } + + } + + return tree; +} + + +static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving) +{ + float newminmax; + int i, k; + + // don't init boudings for the moving case + if(!moving) + { + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + node->bv[2*i] = FLT_MAX; + node->bv[2*i + 1] = -FLT_MAX; + } + } + + for(k = 0; k < numpoints; k++) + { + // for all Axes. + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + newminmax = INPR(&co[k * 3], KDOP_AXES[i]); + if (newminmax < node->bv[2 * i]) + node->bv[2 * i] = newminmax; + if (newminmax > node->bv[(2 * i) + 1]) + node->bv[(2 * i) + 1] = newminmax; + } + } +} + +// depends on the fact that the BVH's for each face is already build +static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end) +{ + float newmin,newmax; + int i, j; + float *bv = node->bv; + + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + bv[2*i] = FLT_MAX; + bv[2*i + 1] = -FLT_MAX; + } + + for (j = start; j < end; j++) + { + // for all Axes. + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + newmin = tree->nodes[j]->bv[(2 * i)]; + if ((newmin < bv[(2 * i)])) + bv[(2 * i)] = newmin; + + newmax = tree->nodes[j]->bv[(2 * i) + 1]; + if ((newmax > bv[(2 * i) + 1])) + bv[(2 * i) + 1] = newmax; + } + } +} + +int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints) +{ + BVHNode *node= NULL; + int i; + + // insert should only possible as long as tree->totbranch is 0 + if(tree->totbranch > 0) + return 0; + + if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)) + return 0; + + // TODO check if have enough nodes in array + + node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]); + tree->totleaf++; + + create_kdop_hull(tree, node, co, numpoints, 0); + + // inflate the bv with some epsilon + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + node->bv[(2 * i)] -= tree->epsilon; // minimum + node->bv[(2 * i) + 1] += tree->epsilon; // maximum + } + + node->index= index; + + return 1; +} + +// only supports x,y,z axis in the moment +// but we should use a plain and simple function here for speed sake +static char get_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 + } +} + +static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char lastaxis) +{ + char laxis; + int i, tend; + BVHNode *tnode; + int slice = (end-start+tree->tree_type-1)/tree->tree_type; //division rounded up + + // Determine which axis to split along + laxis = get_largest_axis(node->bv); + + // split nodes along longest axis + for (i=0; start < end; start += slice, i++) //i counts the current child + { + tend = start + slice; + + if(tend > end) tend = end; + + if(tend-start == 1) // ok, we have 1 left for this node + { + node->children[i] = tree->nodes[start]; + node->children[i]->parent = node; + } + else + { + tnode = node->children[i] = tree->nodes[tree->totleaf + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]); + tree->totbranch++; + tnode->parent = node; + + if(tend != end) + partition_nth_element(tree->nodes, start, end, tend, laxis); + refit_kdop_hull(tree, tnode, start, tend); + bvh_div_nodes(tree, tnode, start, tend, laxis); + } + node->totnode++; + } + + return; +} + +static void verify_tree(BVHTree *tree) +{ + int i, j, check = 0; + + // check the pointer list + for(i = 0; i < tree->totleaf; i++) + { + if(tree->nodes[i]->parent == NULL) + printf("Leaf has no parent: %d\n", i); + else + { + for(j = 0; j < tree->tree_type; j++) + { + if(tree->nodes[i]->parent->children[j] == tree->nodes[i]) + check = 1; + } + if(!check) + { + printf("Parent child relationship doesn't match: %d\n", i); + } + check = 0; + } + } + + // check the leaf list + for(i = 0; i < tree->totleaf; i++) + { + if(tree->nodearray[i].parent == NULL) + printf("Leaf has no parent: %d\n", i); + else + { + for(j = 0; j < tree->tree_type; j++) + { + if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i]) + check = 1; + } + if(!check) + { + printf("Parent child relationship doesn't match: %d\n", i); + } + check = 0; + } + } + + printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf); +} + +void BLI_bvhtree_balance(BVHTree *tree) +{ + BVHNode *node; + + if(tree->totleaf == 0) + return; + + // create root node + node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]); + tree->totbranch++; + + // refit root bvh node + refit_kdop_hull(tree, tree->nodes[tree->totleaf], 0, tree->totleaf); + // create + balance tree + bvh_div_nodes(tree, tree->nodes[tree->totleaf], 0, tree->totleaf, 0); + + // verify_tree(tree); +} + +// overlap - is it possbile for 2 bv's to collide ? +static int tree_overlap(float *bv1, float *bv2, int start_axis, int stop_axis) +{ + float *bv1_end = bv1 + (stop_axis<<1); + + bv1 += start_axis<<1; + bv2 += start_axis<<1; + + // test all axis if min + max overlap + for (; bv1 != bv1_end; bv1+=2, bv2+=2) + { + if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1))) + return 0; + } + + return 1; +} + +static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2) +{ + int j; + + if(tree_overlap(node1->bv, node2->bv, MIN2(data->tree1->start_axis, data->tree2->start_axis), MIN2(data->tree1->stop_axis, data->tree2->stop_axis))) + { + // check if node1 is a leaf + if(!node1->totnode) + { + // check if node2 is a leaf + if(!node2->totnode) + { + + if(node1 == node2) + { + return; + } + + if(data->i >= data->max_overlap) + { + // try to make alloc'ed memory bigger + data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2); + + if(!data->overlap) + { + printf("Out of Memory in traverse\n"); + return; + } + data->max_overlap *= 2; + } + + // both leafs, insert overlap! + data->overlap[data->i].indexA = node1->index; + data->overlap[data->i].indexB = node2->index; + + data->i++; + } + else + { + for(j = 0; j < data->tree2->tree_type; j++) + { + if(node2->children[j]) + traverse(data, node1, node2->children[j]); + } + } + } + else + { + + for(j = 0; j < data->tree2->tree_type; j++) + { + if(node1->children[j]) + traverse(data, node1->children[j], node2); + } + } + } + return; +} + +BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result) +{ + int j, total = 0; + BVHTreeOverlap *overlap = NULL, *to = NULL; + BVHOverlapData **data; + + // check for compatibility of both trees (can't compare 14-DOP with 18-DOP) + if((tree1->axis != tree2->axis) && ((tree1->axis == 14) || tree2->axis == 14)) + return 0; + + // fast check root nodes for collision before doing big splitting + traversal + if(!tree_overlap(tree1->nodes[tree1->totleaf]->bv, tree2->nodes[tree2->totleaf]->bv, MIN2(tree1->start_axis, tree2->start_axis), MIN2(tree1->stop_axis, tree2->stop_axis))) + return 0; + + data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star"); + + for(j = 0; j < tree1->tree_type; j++) + { + data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData"); + + // init BVHOverlapData + data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf)); + data[j]->tree1 = tree1; + data[j]->tree2 = tree2; + data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf); + data[j]->i = 0; + } + +#pragma omp parallel for private(j) schedule(static) + for(j = 0; j < tree1->tree_type; j++) + { + traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]); + } + + for(j = 0; j < tree1->tree_type; j++) + total += data[j]->i; + + to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap"); + + for(j = 0; j < tree1->tree_type; j++) + { + memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap)); + to+=data[j]->i; + } + + for(j = 0; j < tree1->tree_type; j++) + { + free(data[j]->overlap); + MEM_freeN(data[j]); + } + MEM_freeN(data); + + (*result) = total; + return overlap; +} + + +// bottom up update of bvh tree: +// join the 4 children here +static void node_join(BVHTree *tree, BVHNode *node) +{ + int i, j; + + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + node->bv[2*i] = FLT_MAX; + node->bv[2*i + 1] = -FLT_MAX; + } + + for (i = 0; i < tree->tree_type; i++) + { + if (node->children[i]) + { + for (j = tree->start_axis; j < tree->stop_axis; j++) + { + // update minimum + if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) + node->bv[(2 * j)] = node->children[i]->bv[(2 * j)]; + + // update maximum + if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1]) + node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1]; + } + } + else + break; + } +} + +// call before BLI_bvhtree_update_tree() +int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints) +{ + BVHNode *node= NULL; + int i = 0; + + // check if index exists + if(index > tree->totleaf) + return 0; + + node = tree->nodearray + index; + + create_kdop_hull(tree, node, co, numpoints, 0); + + if(co_moving) + create_kdop_hull(tree, node, co_moving, numpoints, 1); + + // inflate the bv with some epsilon + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + node->bv[(2 * i)] -= tree->epsilon; // minimum + node->bv[(2 * i) + 1] += tree->epsilon; // maximum + } + + return 1; +} + +// call BLI_bvhtree_update_node() first for every node/point/triangle +void BLI_bvhtree_update_tree(BVHTree *tree) +{ + BVHNode *leaf, *parent; + + // reset tree traversing flag + for (leaf = tree->nodearray + tree->totleaf; leaf != tree->nodearray + tree->totleaf + tree->totbranch; leaf++) + leaf->traversed = 0; + + for (leaf = tree->nodearray; leaf != tree->nodearray + tree->totleaf; leaf++) + { + for (parent = leaf->parent; parent; parent = parent->parent) + { + parent->traversed++; // we tried to go up in hierarchy + if (parent->traversed < parent->totnode) + break; // we do not need to check further + else + node_join(tree, parent); + } + } +} + +float BLI_bvhtree_getepsilon(BVHTree *tree) +{ + return tree->epsilon; +} diff --git a/source/blender/blenloader/intern/readfile.c b/source/blender/blenloader/intern/readfile.c index 1cb8b10b087..7d0dd9e41c1 100644 --- a/source/blender/blenloader/intern/readfile.c +++ b/source/blender/blenloader/intern/readfile.c @@ -3108,7 +3108,7 @@ static void direct_link_modifiers(FileData *fd, ListBase *lb) collmd->current_v = NULL; collmd->time = -1; collmd->numverts = 0; - collmd->bvh = NULL; + collmd->bvhtree = NULL; collmd->mfaces = NULL; } diff --git a/source/blender/makesdna/DNA_modifier_types.h b/source/blender/makesdna/DNA_modifier_types.h index b7b43817474..fc015775f49 100644 --- a/source/blender/makesdna/DNA_modifier_types.h +++ b/source/blender/makesdna/DNA_modifier_types.h @@ -390,7 +390,7 @@ typedef struct CollisionModifierData { unsigned int numfaces; int pad; float time; /* cfra time of modifier */ - struct BVH *bvh; /* bounding volume hierarchy for this cloth object */ + struct BVHTree *bvhtree; /* bounding volume hierarchy for this cloth object */ } CollisionModifierData; typedef enum { |