diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody/btSparseSDF.h')
| -rw-r--r-- | extern/bullet2/src/BulletSoftBody/btSparseSDF.h | 416 |
1 files changed, 235 insertions, 181 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h index 8992ddbb68d..d611726bcdc 100644 --- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h @@ -20,299 +20,353 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -// Modified Paul Hsieh hash -template <const int DWORDLEN> -unsigned int HsiehHash(const void* pdata) +// Fast Hash + +#if !defined(get16bits) +#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0])) +#endif +// +// super hash function by Paul Hsieh +// +inline unsigned int HsiehHash(const char* data, int len) { - const unsigned short* data=(const unsigned short*)pdata; - unsigned hash=DWORDLEN<<2,tmp; - for(int i=0;i<DWORDLEN;++i) + unsigned int hash = len, tmp; + len >>= 2; + + /* Main loop */ + for (; len > 0; len--) { - hash += data[0]; - tmp = (data[1]<<11)^hash; - hash = (hash<<16)^tmp; - data += 2; - hash += hash>>11; + hash += get16bits(data); + tmp = (get16bits(data + 2) << 11) ^ hash; + hash = (hash << 16) ^ tmp; + data += 2 * sizeof(unsigned short); + hash += hash >> 11; } - hash^=hash<<3;hash+=hash>>5; - hash^=hash<<4;hash+=hash>>17; - hash^=hash<<25;hash+=hash>>6; - return(hash); + + /* Force "avalanching" of final 127 bits */ + hash ^= hash << 3; + hash += hash >> 5; + hash ^= hash << 4; + hash += hash >> 17; + hash ^= hash << 25; + hash += hash >> 6; + + return hash; } template <const int CELLSIZE> -struct btSparseSdf +struct btSparseSdf { // // Inner types // struct IntFrac { - int b; - int i; - btScalar f; + int b; + int i; + btScalar f; }; - struct Cell + struct Cell { - btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; - int c[3]; - int puid; - unsigned hash; - const btCollisionShape* pclient; - Cell* next; + btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1]; + int c[3]; + int puid; + unsigned hash; + const btCollisionShape* pclient; + Cell* next; }; // // Fields // - btAlignedObjectArray<Cell*> cells; - btScalar voxelsz; - int puid; - int ncells; - int m_clampCells; - int nprobes; - int nqueries; + btAlignedObjectArray<Cell*> cells; + btScalar voxelsz; + btScalar m_defaultVoxelsz; + int puid; + int ncells; + int m_clampCells; + int nprobes; + int nqueries; + ~btSparseSdf() + { + Reset(); + } // // 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 //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) m_clampCells = clampCells; - cells.resize(hashsize,0); + cells.resize(hashsize, 0); + m_defaultVoxelsz = 0.25; Reset(); } // - void Reset() + + void setDefaultVoxelsz(btScalar sz) + { + m_defaultVoxelsz = sz; + } + + 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]; - cells[i]=0; - while(pc) + Cell* pc = cells[i]; + cells[i] = 0; + while (pc) { - Cell* pn=pc->next; + Cell* pn = pc->next; delete pc; - pc=pn; + pc = pn; } } - voxelsz =0.25; - puid =0; - ncells =0; - nprobes =1; - nqueries =1; + voxelsz = m_defaultVoxelsz; + puid = 0; + ncells = 0; + nprobes = 1; + nqueries = 1; } // - void GarbageCollect(int lifetime=256) + void GarbageCollect(int lifetime = 256) { - const int life=puid-lifetime; - for(int i=0;i<cells.size();++i) + const int life = puid - lifetime; + for (int i = 0; i < cells.size(); ++i) { - Cell*& root=cells[i]; - Cell* pp=0; - Cell* pc=root; - while(pc) + Cell*& root = cells[i]; + Cell* pp = 0; + Cell* pc = root; + while (pc) { - Cell* pn=pc->next; - if(pc->puid<life) + Cell* pn = pc->next; + if (pc->puid < life) { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;--ncells; + if (pp) + 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); - nqueries=1; - nprobes=1; - ++puid; ///@todo: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ + nqueries = 1; + nprobes = 1; + ++puid; ///@todo: Reset puid's when int range limit is reached */ + /* else setup a priority list... */ } // - int RemoveReferences(btCollisionShape* pcs) + int RemoveReferences(btCollisionShape* pcs) { - int refcount=0; - for(int i=0;i<cells.size();++i) + int refcount = 0; + for (int i = 0; i < cells.size(); ++i) { - Cell*& root=cells[i]; - Cell* pp=0; - Cell* pc=root; - while(pc) + Cell*& root = cells[i]; + Cell* pp = 0; + Cell* pc = root; + while (pc) { - Cell* pn=pc->next; - if(pc->pclient==pcs) + Cell* pn = pc->next; + if (pc->pclient == pcs) { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;++refcount; + if (pp) + 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, - const btCollisionShape* shape, - btVector3& normal, - btScalar margin) + btScalar Evaluate(const btVector3& x, + const btCollisionShape* shape, + btVector3& normal, + btScalar margin) { - /* Lookup cell */ - const btVector3 scx=x/voxelsz; - const IntFrac ix=Decompose(scx.x()); - const IntFrac iy=Decompose(scx.y()); - const IntFrac iz=Decompose(scx.z()); - const unsigned h=Hash(ix.b,iy.b,iz.b,shape); - Cell*& root=cells[static_cast<int>(h%cells.size())]; - Cell* c=root; + /* Lookup cell */ + const btVector3 scx = x / voxelsz; + const IntFrac ix = Decompose(scx.x()); + const IntFrac iy = Decompose(scx.y()); + const IntFrac iz = Decompose(scx.z()); + const unsigned h = Hash(ix.b, iy.b, iz.b, shape); + Cell*& root = cells[static_cast<int>(h % cells.size())]; + Cell* c = root; ++nqueries; - while(c) + while (c) { ++nprobes; - if( (c->hash==h) && - (c->c[0]==ix.b) && - (c->c[1]==iy.b) && - (c->c[2]==iz.b) && - (c->pclient==shape)) - { break; } + if ((c->hash == h) && + (c->c[0] == ix.b) && + (c->c[1] == iy.b) && + (c->c[2] == iz.b) && + (c->pclient == shape)) + { + break; + } 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; - if (ncells>m_clampCells) + //int sz = sizeof(Cell); + if (ncells > m_clampCells) { - static int numResets=0; + static int numResets = 0; numResets++; -// printf("numResets=%d\n",numResets); + // printf("numResets=%d\n",numResets); Reset(); } - c=new Cell(); - c->next=root;root=c; - c->pclient=shape; - c->hash=h; - c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; + c = new Cell(); + c->next = root; + root = c; + c->pclient = shape; + c->hash = h; + c->c[0] = ix.b; + c->c[1] = iy.b; + c->c[2] = iz.b; BuildCell(*c); } - c->puid=puid; - /* Extract infos */ - const int o[]={ ix.i,iy.i,iz.i}; - 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]+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]+1][o[1]+0][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]}; - /* Normal */ + c->puid = puid; + /* Extract infos */ + const int o[] = {ix.i, iy.i, iz.i}; + 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] + 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] + 1][o[1] + 0][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]}; + /* Normal */ #if 1 - const btScalar gx[]={ d[1]-d[0],d[2]-d[3], - d[5]-d[4],d[6]-d[7]}; - const btScalar gy[]={ d[3]-d[0],d[2]-d[1], - d[7]-d[4],d[6]-d[5]}; - const btScalar gz[]={ d[4]-d[0],d[5]-d[1], - d[7]-d[3],d[6]-d[2]}; - normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), - Lerp(gx[2],gx[3],iy.f),iz.f)); - normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), - Lerp(gy[2],gy[3],ix.f),iz.f)); - normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), - Lerp(gz[2],gz[3],ix.f),iy.f)); - normal = normal.normalized(); + const btScalar gx[] = {d[1] - d[0], d[2] - d[3], + d[5] - d[4], d[6] - d[7]}; + const btScalar gy[] = {d[3] - d[0], d[2] - d[1], + d[7] - d[4], d[6] - d[5]}; + const btScalar gz[] = {d[4] - d[0], d[5] - d[1], + d[7] - d[3], d[6] - d[2]}; + normal.setX(Lerp(Lerp(gx[0], gx[1], iy.f), + Lerp(gx[2], gx[3], iy.f), iz.f)); + normal.setY(Lerp(Lerp(gy[0], gy[1], ix.f), + Lerp(gy[2], gy[3], ix.f), iz.f)); + normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f), + Lerp(gz[2], gz[3], ix.f), iy.f)); + normal.safeNormalize(); #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 - /* Distance */ - const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), - Lerp(d[3],d[2],ix.f),iy.f); - const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), - Lerp(d[7],d[6],ix.f),iy.f); - return(Lerp(d0,d1,iz.f)-margin); + /* Distance */ + const btScalar d0 = Lerp(Lerp(d[0], d[1], ix.f), + Lerp(d[3], d[2], ix.f), iy.f); + const btScalar d1 = Lerp(Lerp(d[4], d[5], ix.f), + Lerp(d[7], d[6], ix.f), iy.f); + return (Lerp(d0, d1, iz.f) - margin); } // - void BuildCell(Cell& c) + void BuildCell(Cell& c) { - const btVector3 org=btVector3( (btScalar)c.c[0], - (btScalar)c.c[1], - (btScalar)c.c[2]) * - CELLSIZE*voxelsz; - for(int k=0;k<=CELLSIZE;++k) + const btVector3 org = btVector3((btScalar)c.c[0], + (btScalar)c.c[1], + (btScalar)c.c[2]) * + CELLSIZE * voxelsz; + for (int k = 0; k <= CELLSIZE; ++k) { - const btScalar z=voxelsz*k+org.z(); - for(int j=0;j<=CELLSIZE;++j) + const btScalar z = voxelsz * k + org.z(); + for (int j = 0; j <= CELLSIZE; ++j) { - const btScalar y=voxelsz*j+org.y(); - for(int i=0;i<=CELLSIZE;++i) + const btScalar y = voxelsz * j + org.y(); + for (int i = 0; i <= CELLSIZE; ++i) { - const btScalar x=voxelsz*i+org.x(); - c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); + const btScalar x = voxelsz * i + org.x(); + c.d[i][j][k] = DistanceToShape(btVector3(x, y, z), + c.pclient); } } } } // - static inline btScalar DistanceToShape(const btVector3& x, - const btCollisionShape* shape) + static inline btScalar DistanceToShape(const btVector3& x, + const btCollisionShape* shape) { - btTransform unit; + btTransform unit; unit.setIdentity(); - if(shape->isConvex()) + if (shape->isConvex()) { - btGjkEpaSolver2::sResults res; - const btConvexShape* csh=static_cast<const btConvexShape*>(shape); - return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); + btGjkEpaSolver2::sResults res; + const btConvexShape* csh = static_cast<const btConvexShape*>(shape); + 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... */ - /* Remove test, faster floor... */ - IntFrac r; - x/=CELLSIZE; - const int o=x<0?(int)(-x+1):0; - x+=o;r.b=(int)x; - const btScalar k=(x-r.b)*CELLSIZE; - r.i=(int)k;r.f=k-r.i;r.b-=o; - return(r); + /* Remove test, faster floor... */ + IntFrac r; + x /= CELLSIZE; + const int o = x < 0 ? (int)(-x + 1) : 0; + x += o; + r.b = (int)x; + const btScalar k = (x - r.b) * CELLSIZE; + r.i = (int)k; + r.f = k - r.i; + r.b -= o; + 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 - { - int x,y,z; + { + int x, y, z, w; void* p; }; 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; - const void* ptr = &myset; - - unsigned int result = HsiehHash<sizeof(btS)/4> (ptr); + myset.x = x; + myset.y = y; + myset.z = z; + myset.w = 0; + myset.p = (void*)shape; + const char* ptr = (const char*)&myset; + unsigned int result = HsiehHash(ptr, sizeof(btS)); return result; } }; - -#endif //BT_SPARSE_SDF_H +#endif //BT_SPARSE_SDF_H |