griefith/test
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Collisions: Commit of collision cleanup, put kdop-bvh structure into BLI_kdopbvh (just like kdtree interface now), huge speedup for selfcollisions, also better normal collisions (merge from cloth branch)

Browse Source
This commit is contained in:
Daniel Genrich
2008-06-03 18:48:54 +00:00
parent 62ca0e07da 74903b77f4
commit c84c0201e1
10 changed files with 2121 additions and 1601 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
source/blender/blenkernel/BKE_cloth.h
View File

@@ -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

92
source/blender/blenkernel/BKE_collision.h
View File

@@ -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
/* COLLISION FLAGS */
typedef enum
{
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
{
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;
////////////////////////////////////////
////////////////////////////////////////
// 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);
////////////////////////////////////////
COLLISION_IN_FUTURE = ( 1 << 1 ),
} COLLISION_FLAGS;
////////////////////////////////////////
// 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 );
/////////////////////////////////////////////////

224
source/blender/blenkernel/intern/cloth.c
View File

@@ -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->numverts)
return 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++)
{
VECCOPY(&co[0*3], verts->xold);
BLI_bvhtree_insert(bvhtree, i, co, 1);
}
// balance tree
BLI_bvhtree_balance(bvhtree);
return bvhtree;
}
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;
cloth = clmd->clothObject;
if(!cloth)
return NULL;
verts = cloth->verts;
mfaces = cloth->mfaces;
// in the moment, return zero if no faces there
if(!cloth->numfaces)
return NULL;
bvh = MEM_callocN(sizeof(BVH), "BVH");
if (bvh == NULL)
// create quadtree with k=26
bvhtree = BLI_bvhtree_new(cloth->numfaces, epsilon, 4, 26);
// fill tree
for(i = 0; i < cloth->numfaces; i++, mfaces++)
{
printf("bvh: Out of memory.\n");
return NULL;
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));
}
// springs = cloth->springs;
// numsprings = cloth->numsprings;
bvh->epsilon = epsilon;
bvh->numfaces = cloth->numfaces;
bvh->mfaces = cloth->mfaces;
bvh->numverts = cloth->numverts;
// balance tree
BLI_bvhtree_balance(bvhtree);
bvh->current_x = MEM_callocN ( sizeof ( MVert ) * bvh->numverts, "bvh->current_x" );
if (bvh->current_x == NULL)
{
printf("bvh: Out of memory.\n");
MEM_freeN(bvh);
return NULL;
}
for(i = 0; i < bvh->numverts; i++)
{
VECCOPY(bvh->current_x[i].co, verts[i].tx);
}
bvh_build (bvh);
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;
bvh_update(bvh, moving);
if(!bvhtree)
return;
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 );

1610
source/blender/blenkernel/intern/collision.c
View File

@@ -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;
BVHTree *tree;
float co[12];
int i;
MFace *tface = mfaces;
bvh = MEM_callocN ( sizeof ( BVH ), "BVH" );
if ( bvh == NULL )
tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
// fill tree
for ( i = 0; i < numfaces; i++, tface++ )
{
printf ( "bvh: Out of memory.\n" );
return NULL;
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 );
BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
}
// in the moment, return zero if no faces there
if ( !numfaces )
return NULL;
// balance tree
BLI_bvhtree_balance ( tree );
bvh->epsilon = epsilon;
bvh->numfaces = numfaces;
bvh->mfaces = mfaces;
// we have no faces, we save seperate points
if ( !mfaces )
{
bvh->numfaces = numverts;
}
bvh->numverts = numverts;
bvh->current_x = MEM_dupallocN ( x );
bvh_build ( bvh );
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 );
if ( xnew )
memcpy ( bvh->current_x, xnew, sizeof ( MVert ) * numverts );
// 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 );
bvh_update ( bvh, moving );
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 );
}
}
/***********************************
@@ -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 )
{
float disc = b * b - 4 * a * c;
if ( disc > 0 )
/**
* gsl_poly_solve_quadratic
*
* copied from GSL
*/
int
gsl_poly_solve_quadratic (double a, double b, double c,
double *x0, double *x1)
{
double disc = b * b - 4 * a * c;
if (a == 0) /* Handle linear case */
{
if ( b == 0 )
if (b == 0)
{
float r = ABS ( 0.5 * sqrt ( disc ) / a );
return 0;
}
else
{
*x0 = -c / b;
return 1;
};
}
if (disc > 0)
{
if (b == 0)
{
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];
// 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];
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];
// 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];
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];
// 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];
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^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;
}
if ( i == 3 )
{
if ( ( face1->v4 ) && ( face2->v4 ) )
{
collpair->ap1 = face1->v3;
collpair->ap2 = face1->v4;
collpair->ap3 = face1->v1;
collpair->bp1 = face2->v3;
collpair->bp2 = face2->v4;
collpair->bp3 = face2->v1;
}
else
i++;
}
// 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 );
#else
// just be sure that we don't add anything
distance = 2.0 * ( epsilon + 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;
VECCOPY ( collpair->normal, collpair->vector );
Normalize ( collpair->normal );
collpair->distance = distance;
BLI_linklist_prepend ( &clmd->coll_parms->collision_list, collpair );
}
else
{
MEM_freeN ( collpair );
}
}
else
{
MEM_freeN ( collpair );
}
}
}
int cloth_are_edges_adjacent ( ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair )
{
Cloth *cloth1 = NULL, *cloth2 = NULL;
ClothVertex *verts1 = NULL, *verts2 = NULL;
float temp[3];
cloth1 = clmd->clothObject;
cloth2 = coll_clmd->clothObject;
verts1 = cloth1->verts;
verts2 = cloth2->verts;
VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold );
if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
return 1;
VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold );
if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
return 1;
VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold );
if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
return 1;
VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold );
if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
return 1;
return 0;
}
void cloth_collision_moving_edges ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
{
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];
cloth1 = clmd->clothObject;
cloth2 = coll_clmd->clothObject;
verts1 = cloth1->verts;
verts2 = cloth2->verts;
face1 = & ( cloth1->mfaces[tree1->tri_index] );
face2 = & ( cloth2->mfaces[tree2->tri_index] );
for ( i = 0; i < 5; i++ )
{
if ( i == 0 )
{
edgecollpair.p11 = face1->v1;
edgecollpair.p12 = face1->v2;
}
else if ( i == 1 )
{
edgecollpair.p11 = face1->v2;
edgecollpair.p12 = face1->v3;
}
else if ( i == 2 )
{
if ( face1->v4 )
{
edgecollpair.p11 = face1->v3;
edgecollpair.p12 = face1->v4;
}
else
{
edgecollpair.p11 = face1->v3;
edgecollpair.p12 = face1->v1;
i+=5; // get out of here after this edge pair is handled
}
break;
}
else if ( i == 3 )
{
if ( face1->v4 )
if ( face1->v4 && face2->v4 )
{
edgecollpair.p11 = face1->v4;
edgecollpair.p12 = face1->v1;
// fill faceA
collpair->ap1 = face1->v1;
collpair->ap2 = face1->v4;
collpair->ap3 = face1->v3;
// fill faceB
collpair->bp1 = face2->v1;
collpair->bp2 = face2->v4;
collpair->bp3 = face2->v3;
}
else
continue;
break;
}
#ifdef WITH_BULLET
// calc distance + normal
distance = plNearestPoints (
verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, collmd->current_x[collpair->bp1].co, collmd->current_x[collpair->bp2].co, collmd->current_x[collpair->bp3].co, collpair->pa,collpair->pb,collpair->vector );
#else
// just be sure that we don't add anything
distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO );
#endif
if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
{
VECCOPY ( collpair->normal, collpair->vector );
Normalize ( collpair->normal );
collpair->distance = distance;
collpair->flag = 0;
}
else
{
edgecollpair.p11 = face1->v3;
edgecollpair.p12 = face1->v1;
float w1, w2, w3, u1, u2, u3;
float v1[3], v2[3], relativeVelocity[3];
// 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 );
// 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)
{
// check for collision in the future
collpair->flag |= COLLISION_IN_FUTURE;
}
}
collpair++;
}
return collpair;
}
int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
{
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 );
for ( j = 0; j < 5; j++ )
cloth1 = clmd->clothObject;
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 );
// 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, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[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 );
// 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 )
{
if ( j == 0 )
// 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 )
{
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 ( face2->v4 )
{
edgecollpair.p21 = face2->v4;
edgecollpair.p22 = face2->v1;
}
else
continue;
}
else
{
edgecollpair.p21 = face2->v3;
edgecollpair.p22 = face2->v1;
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 );
if ( !cloth_are_edges_adjacent ( clmd, coll_clmd, &edgecollpair ) )
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 ) )
{
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 );
repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution );
// stay on the safe side and clamp repulse
if ( impulse > ALMOST_ZERO )
repulse = MIN2 ( repulse, 5.0*impulse );
repulse = MAX2 ( impulse, repulse );
for ( k = 0; k < numsolutions; k++ )
{
if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) )
{
//float out_collisionTime = solution[k];
// TODO: check for collisions
// TODO: put into (edge) collision list
// printf("Moving edge found!\n");
}
}
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;
}
static float projectPointOntoLine(float *p, float *a, float *b)
{
float ba[3], pa[3];
VECSUB(ba, b, a);
VECSUB(pa, p, a);
return INPR(pa, ba) / INPR(ba, ba);
}
static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal)
{
float line1[3], line2[3];
float length;
VECSUB(line1, np2, np1);
VECSUB(line2, np3, np1);
// printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]);
Crossf(out_normal, line1, line2);
length = Normalize(out_normal);
if (length <= FLT_EPSILON)
{ // lines are collinear
VECSUB(out_normal, np2, np1);
Normalize(out_normal);
}
}
void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2)
{
CollPair collpair;
Cloth *cloth1=NULL, *cloth2=NULL;
MFace *face1=NULL, *face2=NULL;
ClothVertex *verts1=NULL, *verts2=NULL;
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)
{
*out_a2 = projectPointOntoLine(np11, np21, np22);
if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON))
{
*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);
}
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))
{
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
{ // == 0.0
*out_a1 = projectPointOntoLine(np21, np11, np12);
if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON))
{
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);
}
}
CLAMP(*out_a1, 0.0, 1.0);
calculateEENormal(np11, np12, np21, np22, out_normal);
if(*out_a1 > .5)
{
if(*out_a2 > .5)
{
VECSUB(temp, np12, np22);
}
else
{
VECSUB(temp, np12, np21);
}
}
else
{
if(*out_a2 > .5)
{
VECSUB(temp, np11, np22);
}
else
{
VECSUB(temp, np11, np21);
}
}
return INPR(temp, temp);
}
else
{
// If the lines aren't parallel (but coplanar) they have to intersect
findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2);
// 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);
}
/*
* 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;
}
int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
{
EdgeCollPair edgecollpair;
Cloth *cloth1=NULL;
ClothVertex *verts1=NULL;
unsigned int i = 0, j = 0, k = 0;
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;
for ( i = 0; i < 2; i++ )
cloth1 = clmd->clothObject;
verts1 = cloth1->verts;
for(i = 0; i < 9; i++)
{
cloth1 = clmd->clothObject;
cloth2 = coll_clmd->clothObject;
// 9 edge - edge possibilities
verts1 = cloth1->verts;
verts2 = cloth2->verts;
face1 = & ( cloth1->mfaces[tree1->tri_index] );
face2 = & ( cloth2->mfaces[tree2->tri_index] );
// check all possible pairs of triangles
if ( i == 0 )
if(i == 0) // cloth edge: 1-2; coll edge: 1-2
{
collpair.ap1 = face1->v1;
collpair.ap2 = face1->v2;
collpair.ap3 = face1->v3;
edgecollpair.p11 = collpair->ap1;
edgecollpair.p12 = collpair->ap2;
collpair.pointsb[0] = face2->v1;
collpair.pointsb[1] = face2->v2;
collpair.pointsb[2] = face2->v3;
collpair.pointsb[3] = face2->v4;
edgecollpair.p21 = collpair->bp1;
edgecollpair.p22 = collpair->bp2;
}
if ( i == 1 )
else if(i == 1) // cloth edge: 1-2; coll edge: 2-3
{
if ( face1->v4 )
{
collpair.ap1 = face1->v3;
collpair.ap2 = face1->v4;
collpair.ap3 = face1->v1;
edgecollpair.p11 = collpair->ap1;
edgecollpair.p12 = collpair->ap2;
collpair.pointsb[0] = face2->v1;
collpair.pointsb[1] = face2->v2;
collpair.pointsb[2] = face2->v3;
collpair.pointsb[3] = face2->v4;
}
else
i++;
edgecollpair.p21 = collpair->bp2;
edgecollpair.p22 = collpair->bp3;
}
// calc SIPcode (?)
if ( i < 2 )
else if(i == 2) // cloth edge: 1-2; coll edge: 1-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->ap2;
for ( j = 0; j < 4; j++ )
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;
edgecollpair.p21 = collpair->bp1;
edgecollpair.p22 = collpair->bp2;
}
else if(i == 4) // cloth edge: 2-3; coll edge: 2-3
{
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;
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;
edgecollpair.p21 = collpair->bp1;
edgecollpair.p22 = collpair->bp2;
}
else if(i ==7) // cloth edge: 1-3; coll edge: 2-3
{
edgecollpair.p11 = collpair->ap1;
edgecollpair.p12 = collpair->ap3;
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;
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 );
VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
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 ( ( j==3 ) && ! ( face2->v4 ) )
break;
VECSUB ( e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold );
VECSUB ( f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v );
numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution );
for ( k = 0; k < numsolutions; k++ )
if(edgecollpair.p21==6 || edgecollpair.p22 == 6)
{
if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) )
{
//float out_collisionTime = solution[k];
// TODO: check for collisions
// TODO: put into (point-face) collision list
// printf("Moving found!\n");
}
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);
}
// TODO: check borders for collisions
}
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))
{
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 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);
VECADD(vrel_1_to_2, vrel_1_to_2, temp);
VECCOPY(temp, verts1[edgecollpair.p21].tv);
VecMulf(temp, 1.0 - b);
VECCOPY(temp2, verts1[edgecollpair.p22].tv);
VecMulf(temp2, b);
VECADD(temp, temp, temp2);
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]);
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++;
}
*/

860
source/blender/blenkernel/intern/kdop.c
View File

@@ -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);
}
}
}
}
}

29
source/blender/blenkernel/intern/modifier.c
View File

@@ -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;

60
source/blender/blenlib/BLI_kdopbvh.h Normal file
View File

@@ -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

811
source/blender/blenlib/intern/BLI_kdopbvh.c Normal file
View File

@@ -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;
}

2
source/blender/blenloader/intern/readfile.c
View File

@@ -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;
}

2
source/blender/makesdna/DNA_modifier_types.h
View File

@@ -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 {