diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSparseSDF.h')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btSparseSDF.h | 81 |
1 files changed, 55 insertions, 26 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h index a52b2cb1cc..eb290a1dbd 100644 --- a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h +++ b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h @@ -20,27 +20,38 @@ 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) -{ - 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 ^= hash << 3; - hash += hash >> 5; - hash ^= hash << 4; - hash += hash >> 17; - hash ^= hash << 25; - hash += hash >> 6; - return (hash); +// 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> @@ -70,12 +81,17 @@ struct btSparseSdf btAlignedObjectArray<Cell*> cells; btScalar voxelsz; + btScalar m_defaultVoxelsz; int puid; int ncells; int m_clampCells; int nprobes; int nqueries; + ~btSparseSdf() + { + Reset(); + } // // Methods // @@ -87,9 +103,16 @@ struct btSparseSdf //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) @@ -103,7 +126,7 @@ struct btSparseSdf pc = pn; } } - voxelsz = 0.25; + voxelsz = m_defaultVoxelsz; puid = 0; ncells = 0; nprobes = 1; @@ -197,6 +220,9 @@ struct btSparseSdf } 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; } } @@ -248,7 +274,7 @@ struct btSparseSdf 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(); + normal.safeNormalize(); #else normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized(); #endif @@ -322,19 +348,22 @@ struct btSparseSdf { 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.w = 0; myset.p = (void*)shape; - const void* ptr = &myset; + const char* ptr = (const char*)&myset; - unsigned int result = HsiehHash<sizeof(btS) / 4>(ptr); + unsigned int result = HsiehHash(ptr, sizeof(btS) ); return result; } |