Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/BulletSoftBody/btSparseSDF.h
| Show All 14 Lines | |||||
| ///btSparseSdf implementation by Nathanael Presson | ///btSparseSdf implementation by Nathanael Presson | ||||
| #ifndef BT_SPARSE_SDF_H | #ifndef BT_SPARSE_SDF_H | ||||
| #define BT_SPARSE_SDF_H | #define BT_SPARSE_SDF_H | ||||
| #include "BulletCollision/CollisionDispatch/btCollisionObject.h" | #include "BulletCollision/CollisionDispatch/btCollisionObject.h" | ||||
| #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" | #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" | ||||
| // Modified Paul Hsieh hash | // Fast Hash | ||||
| template <const int DWORDLEN> | |||||
| unsigned int HsiehHash(const void* pdata) | #if !defined(get16bits) | ||||
| { | #define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0])) | ||||
| const unsigned short* data=(const unsigned short*)pdata; | #endif | ||||
| unsigned hash=DWORDLEN<<2,tmp; | // | ||||
| for(int i=0;i<DWORDLEN;++i) | // super hash function by Paul Hsieh | ||||
| // | |||||
| inline unsigned int HsiehHash(const char* data, int len) | |||||
| { | { | ||||
| hash += data[0]; | unsigned int hash = len, tmp; | ||||
| tmp = (data[1]<<11)^hash; | len >>= 2; | ||||
| /* Main loop */ | |||||
| for (; len > 0; len--) | |||||
| { | |||||
| hash += get16bits(data); | |||||
| tmp = (get16bits(data + 2) << 11) ^ hash; | |||||
| hash = (hash<<16)^tmp; | hash = (hash << 16) ^ tmp; | ||||
| data += 2; | data += 2 * sizeof(unsigned short); | ||||
| hash += hash>>11; | hash += hash >> 11; | ||||
| } | } | ||||
| hash^=hash<<3;hash+=hash>>5; | |||||
| hash^=hash<<4;hash+=hash>>17; | /* Force "avalanching" of final 127 bits */ | ||||
| hash^=hash<<25;hash+=hash>>6; | hash ^= hash << 3; | ||||
| return(hash); | hash += hash >> 5; | ||||
| hash ^= hash << 4; | |||||
| hash += hash >> 17; | |||||
| hash ^= hash << 25; | |||||
| hash += hash >> 6; | |||||
| return hash; | |||||
| } | } | ||||
| template <const int CELLSIZE> | template <const int CELLSIZE> | ||||
| struct btSparseSdf | struct btSparseSdf | ||||
| { | { | ||||
| // | // | ||||
| // Inner types | // Inner types | ||||
| // | // | ||||
| struct IntFrac | struct IntFrac | ||||
| { | { | ||||
| int b; | int b; | ||||
| int i; | int i; | ||||
| btScalar f; | btScalar f; | ||||
| }; | }; | ||||
| struct Cell | struct Cell | ||||
| { | { | ||||
| btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; | btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1]; | ||||
| int c[3]; | int c[3]; | ||||
| int puid; | int puid; | ||||
| unsigned hash; | unsigned hash; | ||||
| const btCollisionShape* pclient; | const btCollisionShape* pclient; | ||||
| Cell* next; | Cell* next; | ||||
| }; | }; | ||||
| // | // | ||||
| // Fields | // Fields | ||||
| // | // | ||||
| btAlignedObjectArray<Cell*> cells; | btAlignedObjectArray<Cell*> cells; | ||||
| btScalar voxelsz; | btScalar voxelsz; | ||||
| btScalar m_defaultVoxelsz; | |||||
| int puid; | int puid; | ||||
| int ncells; | int ncells; | ||||
| int m_clampCells; | int m_clampCells; | ||||
| int nprobes; | int nprobes; | ||||
| int nqueries; | int nqueries; | ||||
| ~btSparseSdf() | |||||
| { | |||||
| Reset(); | |||||
| } | |||||
| // | // | ||||
| // Methods | // Methods | ||||
| // | // | ||||
| // | // | ||||
| void Initialize(int hashsize=2383, int clampCells = 256*1024) | void Initialize(int hashsize = 2383, int clampCells = 256 * 1024) | ||||
| { | { | ||||
| //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated | //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated | ||||
| //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) | //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) | ||||
| m_clampCells = clampCells; | m_clampCells = clampCells; | ||||
| cells.resize(hashsize,0); | cells.resize(hashsize, 0); | ||||
| m_defaultVoxelsz = 0.25; | |||||
| Reset(); | Reset(); | ||||
| } | } | ||||
| // | // | ||||
| void setDefaultVoxelsz(btScalar sz) | |||||
| { | |||||
| m_defaultVoxelsz = sz; | |||||
| } | |||||
| void Reset() | void Reset() | ||||
| { | { | ||||
| for(int i=0,ni=cells.size();i<ni;++i) | for (int i = 0, ni = cells.size(); i < ni; ++i) | ||||
| { | { | ||||
| Cell* pc=cells[i]; | Cell* pc = cells[i]; | ||||
| cells[i]=0; | cells[i] = 0; | ||||
| while(pc) | while (pc) | ||||
| { | { | ||||
| Cell* pn=pc->next; | Cell* pn = pc->next; | ||||
| delete pc; | delete pc; | ||||
| pc=pn; | pc = pn; | ||||
| } | } | ||||
| } | } | ||||
| voxelsz =0.25; | voxelsz = m_defaultVoxelsz; | ||||
| puid =0; | puid = 0; | ||||
| ncells =0; | ncells = 0; | ||||
| nprobes =1; | nprobes = 1; | ||||
| nqueries =1; | nqueries = 1; | ||||
| } | } | ||||
| // | // | ||||
| void GarbageCollect(int lifetime=256) | void GarbageCollect(int lifetime = 256) | ||||
| { | { | ||||
| const int life=puid-lifetime; | const int life = puid - lifetime; | ||||
| for(int i=0;i<cells.size();++i) | for (int i = 0; i < cells.size(); ++i) | ||||
| { | { | ||||
| Cell*& root=cells[i]; | Cell*& root = cells[i]; | ||||
| Cell* pp=0; | Cell* pp = 0; | ||||
| Cell* pc=root; | Cell* pc = root; | ||||
| while(pc) | while (pc) | ||||
| { | { | ||||
| Cell* pn=pc->next; | Cell* pn = pc->next; | ||||
| if(pc->puid<life) | if (pc->puid < life) | ||||
| { | { | ||||
| if(pp) pp->next=pn; else root=pn; | if (pp) | ||||
| delete pc;pc=pp;--ncells; | pp->next = pn; | ||||
| else | |||||
| root = pn; | |||||
| delete pc; | |||||
| pc = pp; | |||||
| --ncells; | |||||
| } | } | ||||
| pp=pc;pc=pn; | pp = pc; | ||||
| pc = pn; | |||||
| } | } | ||||
| } | } | ||||
| //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); | //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); | ||||
| nqueries=1; | nqueries = 1; | ||||
| nprobes=1; | nprobes = 1; | ||||
| ++puid; ///@todo: Reset puid's when int range limit is reached */ | ++puid; ///@todo: Reset puid's when int range limit is reached */ | ||||
| /* else setup a priority list... */ | /* else setup a priority list... */ | ||||
| } | } | ||||
| // | // | ||||
| int RemoveReferences(btCollisionShape* pcs) | int RemoveReferences(btCollisionShape* pcs) | ||||
| { | { | ||||
| int refcount=0; | int refcount = 0; | ||||
| for(int i=0;i<cells.size();++i) | for (int i = 0; i < cells.size(); ++i) | ||||
| { | { | ||||
| Cell*& root=cells[i]; | Cell*& root = cells[i]; | ||||
| Cell* pp=0; | Cell* pp = 0; | ||||
| Cell* pc=root; | Cell* pc = root; | ||||
| while(pc) | while (pc) | ||||
| { | { | ||||
| Cell* pn=pc->next; | Cell* pn = pc->next; | ||||
| if(pc->pclient==pcs) | if (pc->pclient == pcs) | ||||
| { | { | ||||
| if(pp) pp->next=pn; else root=pn; | if (pp) | ||||
| delete pc;pc=pp;++refcount; | pp->next = pn; | ||||
| else | |||||
| root = pn; | |||||
| delete pc; | |||||
| pc = pp; | |||||
| ++refcount; | |||||
| } | } | ||||
| pp=pc;pc=pn; | pp = pc; | ||||
| pc = pn; | |||||
| } | } | ||||
| } | } | ||||
| return(refcount); | return (refcount); | ||||
| } | } | ||||
| // | // | ||||
| btScalar Evaluate( const btVector3& x, | btScalar Evaluate(const btVector3& x, | ||||
| const btCollisionShape* shape, | const btCollisionShape* shape, | ||||
| btVector3& normal, | btVector3& normal, | ||||
| btScalar margin) | btScalar margin) | ||||
| { | { | ||||
| /* Lookup cell */ | /* Lookup cell */ | ||||
| const btVector3 scx=x/voxelsz; | const btVector3 scx = x / voxelsz; | ||||
| const IntFrac ix=Decompose(scx.x()); | const IntFrac ix = Decompose(scx.x()); | ||||
| const IntFrac iy=Decompose(scx.y()); | const IntFrac iy = Decompose(scx.y()); | ||||
| const IntFrac iz=Decompose(scx.z()); | const IntFrac iz = Decompose(scx.z()); | ||||
| const unsigned h=Hash(ix.b,iy.b,iz.b,shape); | const unsigned h = Hash(ix.b, iy.b, iz.b, shape); | ||||
| Cell*& root=cells[static_cast<int>(h%cells.size())]; | Cell*& root = cells[static_cast<int>(h % cells.size())]; | ||||
| Cell* c=root; | Cell* c = root; | ||||
| ++nqueries; | ++nqueries; | ||||
| while(c) | while (c) | ||||
| { | { | ||||
| ++nprobes; | ++nprobes; | ||||
| if( (c->hash==h) && | if ((c->hash == h) && | ||||
| (c->c[0]==ix.b) && | (c->c[0] == ix.b) && | ||||
| (c->c[1]==iy.b) && | (c->c[1] == iy.b) && | ||||
| (c->c[2]==iz.b) && | (c->c[2] == iz.b) && | ||||
| (c->pclient==shape)) | (c->pclient == shape)) | ||||
| { break; } | { | ||||
| break; | |||||
| } | |||||
| else | else | ||||
| { c=c->next; } | { | ||||
| // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]); | |||||
| //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); | |||||
| c = c->next; | |||||
| } | |||||
| } | } | ||||
| if(!c) | if (!c) | ||||
| { | { | ||||
| ++nprobes; | ++nprobes; | ||||
| ++ncells; | ++ncells; | ||||
| //int sz = sizeof(Cell); | |||||
| if (ncells>m_clampCells) | if (ncells > m_clampCells) | ||||
| { | { | ||||
| static int numResets=0; | static int numResets = 0; | ||||
| numResets++; | numResets++; | ||||
| // printf("numResets=%d\n",numResets); | // printf("numResets=%d\n",numResets); | ||||
| Reset(); | Reset(); | ||||
| } | } | ||||
| c=new Cell(); | c = new Cell(); | ||||
| c->next=root;root=c; | c->next = root; | ||||
| root = c; | |||||
| c->pclient=shape; | c->pclient = shape; | ||||
| c->hash=h; | c->hash = h; | ||||
| c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; | c->c[0] = ix.b; | ||||
| c->c[1] = iy.b; | |||||
| c->c[2] = iz.b; | |||||
| BuildCell(*c); | BuildCell(*c); | ||||
| } | } | ||||
| c->puid=puid; | c->puid = puid; | ||||
| /* Extract infos */ | /* Extract infos */ | ||||
| const int o[]={ ix.i,iy.i,iz.i}; | const int o[] = {ix.i, iy.i, iz.i}; | ||||
| const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], | const btScalar d[] = {c->d[o[0] + 0][o[1] + 0][o[2] + 0], | ||||
| c->d[o[0]+1][o[1]+0][o[2]+0], | c->d[o[0] + 1][o[1] + 0][o[2] + 0], | ||||
| c->d[o[0]+1][o[1]+1][o[2]+0], | c->d[o[0] + 1][o[1] + 1][o[2] + 0], | ||||
| c->d[o[0]+0][o[1]+1][o[2]+0], | c->d[o[0] + 0][o[1] + 1][o[2] + 0], | ||||
| c->d[o[0]+0][o[1]+0][o[2]+1], | c->d[o[0] + 0][o[1] + 0][o[2] + 1], | ||||
| c->d[o[0]+1][o[1]+0][o[2]+1], | c->d[o[0] + 1][o[1] + 0][o[2] + 1], | ||||
| c->d[o[0]+1][o[1]+1][o[2]+1], | c->d[o[0] + 1][o[1] + 1][o[2] + 1], | ||||
| c->d[o[0]+0][o[1]+1][o[2]+1]}; | c->d[o[0] + 0][o[1] + 1][o[2] + 1]}; | ||||
| /* Normal */ | /* Normal */ | ||||
| #if 1 | #if 1 | ||||
| const btScalar gx[]={ d[1]-d[0],d[2]-d[3], | const btScalar gx[] = {d[1] - d[0], d[2] - d[3], | ||||
| d[5]-d[4],d[6]-d[7]}; | d[5] - d[4], d[6] - d[7]}; | ||||
| const btScalar gy[]={ d[3]-d[0],d[2]-d[1], | const btScalar gy[] = {d[3] - d[0], d[2] - d[1], | ||||
| d[7]-d[4],d[6]-d[5]}; | d[7] - d[4], d[6] - d[5]}; | ||||
| const btScalar gz[]={ d[4]-d[0],d[5]-d[1], | const btScalar gz[] = {d[4] - d[0], d[5] - d[1], | ||||
| d[7]-d[3],d[6]-d[2]}; | d[7] - d[3], d[6] - d[2]}; | ||||
| normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), | normal.setX(Lerp(Lerp(gx[0], gx[1], iy.f), | ||||
| Lerp(gx[2],gx[3],iy.f),iz.f)); | Lerp(gx[2], gx[3], iy.f), iz.f)); | ||||
| normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), | normal.setY(Lerp(Lerp(gy[0], gy[1], ix.f), | ||||
| Lerp(gy[2],gy[3],ix.f),iz.f)); | Lerp(gy[2], gy[3], ix.f), iz.f)); | ||||
| normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), | normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f), | ||||
| Lerp(gz[2],gz[3],ix.f),iy.f)); | Lerp(gz[2], gz[3], ix.f), iy.f)); | ||||
| normal = normal.normalized(); | normal.safeNormalize(); | ||||
| #else | #else | ||||
| normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); | normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized(); | ||||
| #endif | #endif | ||||
| /* Distance */ | /* Distance */ | ||||
| const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), | const btScalar d0 = Lerp(Lerp(d[0], d[1], ix.f), | ||||
| Lerp(d[3],d[2],ix.f),iy.f); | Lerp(d[3], d[2], ix.f), iy.f); | ||||
| const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), | const btScalar d1 = Lerp(Lerp(d[4], d[5], ix.f), | ||||
| Lerp(d[7],d[6],ix.f),iy.f); | Lerp(d[7], d[6], ix.f), iy.f); | ||||
| return(Lerp(d0,d1,iz.f)-margin); | return (Lerp(d0, d1, iz.f) - margin); | ||||
| } | } | ||||
| // | // | ||||
| void BuildCell(Cell& c) | void BuildCell(Cell& c) | ||||
| { | { | ||||
| const btVector3 org=btVector3( (btScalar)c.c[0], | const btVector3 org = btVector3((btScalar)c.c[0], | ||||
| (btScalar)c.c[1], | (btScalar)c.c[1], | ||||
| (btScalar)c.c[2]) * | (btScalar)c.c[2]) * | ||||
| CELLSIZE*voxelsz; | CELLSIZE * voxelsz; | ||||
| for(int k=0;k<=CELLSIZE;++k) | for (int k = 0; k <= CELLSIZE; ++k) | ||||
| { | { | ||||
| const btScalar z=voxelsz*k+org.z(); | const btScalar z = voxelsz * k + org.z(); | ||||
| for(int j=0;j<=CELLSIZE;++j) | for (int j = 0; j <= CELLSIZE; ++j) | ||||
| { | { | ||||
| const btScalar y=voxelsz*j+org.y(); | const btScalar y = voxelsz * j + org.y(); | ||||
| for(int i=0;i<=CELLSIZE;++i) | for (int i = 0; i <= CELLSIZE; ++i) | ||||
| { | { | ||||
| const btScalar x=voxelsz*i+org.x(); | const btScalar x = voxelsz * i + org.x(); | ||||
| c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), | c.d[i][j][k] = DistanceToShape(btVector3(x, y, z), | ||||
| c.pclient); | c.pclient); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| // | // | ||||
| static inline btScalar DistanceToShape(const btVector3& x, | static inline btScalar DistanceToShape(const btVector3& x, | ||||
| const btCollisionShape* shape) | const btCollisionShape* shape) | ||||
| { | { | ||||
| btTransform unit; | btTransform unit; | ||||
| unit.setIdentity(); | unit.setIdentity(); | ||||
| if(shape->isConvex()) | if (shape->isConvex()) | ||||
| { | { | ||||
| btGjkEpaSolver2::sResults res; | btGjkEpaSolver2::sResults res; | ||||
| const btConvexShape* csh=static_cast<const btConvexShape*>(shape); | const btConvexShape* csh = static_cast<const btConvexShape*>(shape); | ||||
| return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); | return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res)); | ||||
| } | } | ||||
| return(0); | return (0); | ||||
| } | } | ||||
| // | // | ||||
| static inline IntFrac Decompose(btScalar x) | static inline IntFrac Decompose(btScalar x) | ||||
| { | { | ||||
| /* That one need a lot of improvements... */ | /* That one need a lot of improvements... */ | ||||
| /* Remove test, faster floor... */ | /* Remove test, faster floor... */ | ||||
| IntFrac r; | IntFrac r; | ||||
| x/=CELLSIZE; | x /= CELLSIZE; | ||||
| const int o=x<0?(int)(-x+1):0; | const int o = x < 0 ? (int)(-x + 1) : 0; | ||||
| x+=o;r.b=(int)x; | x += o; | ||||
| r.b = (int)x; | |||||
| const btScalar k=(x-r.b)*CELLSIZE; | const btScalar k = (x - r.b) * CELLSIZE; | ||||
| r.i=(int)k;r.f=k-r.i;r.b-=o; | r.i = (int)k; | ||||
| r.f = k - r.i; | |||||
| r.b -= o; | |||||
| return(r); | return (r); | ||||
| } | } | ||||
| // | // | ||||
| static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) | static inline btScalar Lerp(btScalar a, btScalar b, btScalar t) | ||||
| { | { | ||||
| return(a+(b-a)*t); | return (a + (b - a) * t); | ||||
| } | } | ||||
| // | // | ||||
| static inline unsigned int Hash(int x,int y,int z,const btCollisionShape* shape) | static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape) | ||||
| { | { | ||||
| struct btS | struct btS | ||||
| { | { | ||||
| int x,y,z; | int x, y, z, w; | ||||
| void* p; | void* p; | ||||
| }; | }; | ||||
| btS myset; | btS myset; | ||||
| //memset may be needed in case of additional (uninitialized) padding! | |||||
| //memset(&myset, 0, sizeof(btS)); | |||||
| myset.x=x;myset.y=y;myset.z=z;myset.p=(void*)shape; | myset.x = x; | ||||
| const void* ptr = &myset; | myset.y = y; | ||||
| myset.z = z; | |||||
| unsigned int result = HsiehHash<sizeof(btS)/4> (ptr); | myset.w = 0; | ||||
| myset.p = (void*)shape; | |||||
| const char* ptr = (const char*)&myset; | |||||
| unsigned int result = HsiehHash(ptr, sizeof(btS)); | |||||
| return result; | return result; | ||||
| } | } | ||||
| }; | }; | ||||
| #endif //BT_SPARSE_SDF_H | #endif //BT_SPARSE_SDF_H | ||||