summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btSparseSDF.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSparseSDF.h')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btSparseSDF.h378
1 files changed, 201 insertions, 177 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h
index ba437c28ef..a52b2cb1cc 100644
--- a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h
+++ b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h
@@ -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
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 21:12:54 +0000