Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
@@ -24,296 +24,320 @@ subject to the following restrictions:
|
||||
template <const int DWORDLEN>
|
||||
unsigned int HsiehHash(const void* pdata)
|
||||
{
|
||||
const unsigned short* data=(const unsigned short*)pdata;
|
||||
unsigned hash=DWORDLEN<<2,tmp;
|
||||
for(int i=0;i<DWORDLEN;++i)
|
||||
const unsigned short* data = (const unsigned short*)pdata;
|
||||
unsigned hash = DWORDLEN << 2, tmp;
|
||||
for (int i = 0; i < DWORDLEN; ++i)
|
||||
{
|
||||
hash += data[0];
|
||||
tmp = (data[1]<<11)^hash;
|
||||
hash = (hash<<16)^tmp;
|
||||
data += 2;
|
||||
hash += hash>>11;
|
||||
hash += data[0];
|
||||
tmp = (data[1] << 11) ^ hash;
|
||||
hash = (hash << 16) ^ tmp;
|
||||
data += 2;
|
||||
hash += hash >> 11;
|
||||
}
|
||||
hash^=hash<<3;hash+=hash>>5;
|
||||
hash^=hash<<4;hash+=hash>>17;
|
||||
hash^=hash<<25;hash+=hash>>6;
|
||||
return(hash);
|
||||
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;
|
||||
int puid;
|
||||
int ncells;
|
||||
int m_clampCells;
|
||||
int nprobes;
|
||||
int nqueries;
|
||||
|
||||
//
|
||||
// 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);
|
||||
Reset();
|
||||
}
|
||||
//
|
||||
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];
|
||||
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 = 0.25;
|
||||
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; }
|
||||
{
|
||||
c = c->next;
|
||||
}
|
||||
}
|
||||
if(!c)
|
||||
if (!c)
|
||||
{
|
||||
++nprobes;
|
||||
++nprobes;
|
||||
++ncells;
|
||||
//int sz = sizeof(Cell);
|
||||
if (ncells>m_clampCells)
|
||||
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 = normal.normalized();
|
||||
#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;
|
||||
void* p;
|
||||
};
|
||||
|
||||
btS myset;
|
||||
|
||||
myset.x=x;myset.y=y;myset.z=z;myset.p=(void*)shape;
|
||||
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);
|
||||
|
||||
unsigned int result = HsiehHash<sizeof(btS) / 4>(ptr);
|
||||
|
||||
return result;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_SPARSE_SDF_H
|
||||
#endif //BT_SPARSE_SDF_H
|
||||
|
||||
Reference in New Issue
Block a user