Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/BulletSoftBody/btSparseSDF.h

Changeset View

Standalone View

View Options

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