123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373 |
- /*
- Bullet Continuous Collision Detection and Physics Library
- Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
- This software is provided 'as-is', without any express or implied warranty.
- In no event will the authors be held liable for any damages arising from the use of this software.
- Permission is granted to anyone to use this software for any purpose,
- including commercial applications, and to alter it and redistribute it freely,
- subject to the following restrictions:
- 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
- 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
- 3. This notice may not be removed or altered from any source distribution.
- */
- ///btSparseSdf implementation by Nathanael Presson
- #ifndef BT_SPARSE_SDF_H
- #define BT_SPARSE_SDF_H
- #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
- #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
- // 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)
- {
- unsigned int hash = len, tmp;
- len >>= 2;
- /* Main loop */
- for (; len > 0; len--)
- {
- hash += get16bits(data);
- tmp = (get16bits(data + 2) << 11) ^ hash;
- hash = (hash << 16) ^ tmp;
- data += 2 * sizeof(unsigned short);
- hash += hash >> 11;
- }
- /* 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
- {
- //
- // Inner types
- //
- struct IntFrac
- {
- int b;
- int i;
- btScalar f;
- };
- struct Cell
- {
- 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;
- 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)
- {
- //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);
- m_defaultVoxelsz = 0.25;
- Reset();
- }
- //
- void setDefaultVoxelsz(btScalar sz)
- {
- m_defaultVoxelsz = sz;
- }
- void Reset()
- {
- for (int i = 0, ni = cells.size(); i < ni; ++i)
- {
- Cell* pc = cells[i];
- cells[i] = 0;
- while (pc)
- {
- Cell* pn = pc->next;
- delete pc;
- pc = pn;
- }
- }
- voxelsz = m_defaultVoxelsz;
- puid = 0;
- ncells = 0;
- nprobes = 1;
- nqueries = 1;
- }
- //
- void GarbageCollect(int lifetime = 256)
- {
- 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* pn = pc->next;
- if (pc->puid < life)
- {
- if (pp)
- pp->next = pn;
- else
- root = pn;
- delete pc;
- pc = pp;
- --ncells;
- }
- 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... */
- }
- //
- int RemoveReferences(btCollisionShape* pcs)
- {
- int refcount = 0;
- for (int i = 0; i < cells.size(); ++i)
- {
- Cell*& root = cells[i];
- Cell* pp = 0;
- Cell* pc = root;
- while (pc)
- {
- Cell* pn = pc->next;
- if (pc->pclient == pcs)
- {
- if (pp)
- pp->next = pn;
- else
- root = pn;
- delete pc;
- pc = pp;
- ++refcount;
- }
- pp = pc;
- pc = pn;
- }
- }
- return (refcount);
- }
- //
- 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;
- ++nqueries;
- 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;
- }
- else
- {
- // 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)
- {
- ++nprobes;
- ++ncells;
- //int sz = sizeof(Cell);
- if (ncells > m_clampCells)
- {
- //static int numResets = 0;
- //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;
- 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 */
- #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.safeNormalize();
- #else
- 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);
- }
- //
- 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 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 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)
- {
- btTransform unit;
- unit.setIdentity();
- if (shape->isConvex())
- {
- btGjkEpaSolver2::sResults res;
- const btConvexShape* csh = static_cast<const btConvexShape*>(shape);
- return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res));
- }
- return (0);
- }
- //
- 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);
- }
- //
- static inline btScalar Lerp(btScalar a, btScalar b, btScalar t)
- {
- return (a + (b - a) * t);
- }
- //
- static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape)
- {
- struct btS
- {
- 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.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
|