2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
15 ///btSparseSdf implementation by Nathanael Presson
17 #ifndef BT_SPARSE_SDF_H
18 #define BT_SPARSE_SDF_H
20 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
21 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
25 #if !defined(get16bits)
26 #define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
29 // super hash function by Paul Hsieh
31 inline unsigned int HsiehHash(const char* data, int len)
33 unsigned int hash = len, tmp;
37 for (; len > 0; len--)
39 hash += get16bits(data);
40 tmp = (get16bits(data + 2) << 11) ^ hash;
41 hash = (hash << 16) ^ tmp;
42 data += 2 * sizeof(unsigned short);
46 /* Force "avalanching" of final 127 bits */
57 template <const int CELLSIZE>
71 btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1];
75 const btCollisionShape* pclient;
82 btAlignedObjectArray<Cell*> cells;
84 btScalar m_defaultVoxelsz;
100 void Initialize(int hashsize = 2383, int clampCells = 256 * 1024)
102 //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated
103 //if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
104 m_clampCells = clampCells;
105 cells.resize(hashsize, 0);
106 m_defaultVoxelsz = 0.25;
111 void setDefaultVoxelsz(btScalar sz)
113 m_defaultVoxelsz = sz;
118 for (int i = 0, ni = cells.size(); i < ni; ++i)
129 voxelsz = m_defaultVoxelsz;
136 void GarbageCollect(int lifetime = 256)
138 const int life = puid - lifetime;
139 for (int i = 0; i < cells.size(); ++i)
141 Cell*& root = cells[i];
161 //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
164 ++puid; ///@todo: Reset puid's when int range limit is reached */
165 /* else setup a priority list... */
168 int RemoveReferences(btCollisionShape* pcs)
171 for (int i = 0; i < cells.size(); ++i)
173 Cell*& root = cells[i];
179 if (pc->pclient == pcs)
196 btScalar Evaluate(const btVector3& x,
197 const btCollisionShape* shape,
202 const btVector3 scx = x / voxelsz;
203 const IntFrac ix = Decompose(scx.x());
204 const IntFrac iy = Decompose(scx.y());
205 const IntFrac iz = Decompose(scx.z());
206 const unsigned h = Hash(ix.b, iy.b, iz.b, shape);
207 Cell*& root = cells[static_cast<int>(h % cells.size())];
213 if ((c->hash == h) &&
217 (c->pclient == shape))
223 // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]);
224 //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
233 //int sz = sizeof(Cell);
234 if (ncells > m_clampCells)
236 //static int numResets = 0;
238 //printf("numResets=%d\n",numResets);
254 const int o[] = {ix.i, iy.i, iz.i};
255 const btScalar d[] = {c->d[o[0] + 0][o[1] + 0][o[2] + 0],
256 c->d[o[0] + 1][o[1] + 0][o[2] + 0],
257 c->d[o[0] + 1][o[1] + 1][o[2] + 0],
258 c->d[o[0] + 0][o[1] + 1][o[2] + 0],
259 c->d[o[0] + 0][o[1] + 0][o[2] + 1],
260 c->d[o[0] + 1][o[1] + 0][o[2] + 1],
261 c->d[o[0] + 1][o[1] + 1][o[2] + 1],
262 c->d[o[0] + 0][o[1] + 1][o[2] + 1]};
265 const btScalar gx[] = {d[1] - d[0], d[2] - d[3],
266 d[5] - d[4], d[6] - d[7]};
267 const btScalar gy[] = {d[3] - d[0], d[2] - d[1],
268 d[7] - d[4], d[6] - d[5]};
269 const btScalar gz[] = {d[4] - d[0], d[5] - d[1],
270 d[7] - d[3], d[6] - d[2]};
271 normal.setX(Lerp(Lerp(gx[0], gx[1], iy.f),
272 Lerp(gx[2], gx[3], iy.f), iz.f));
273 normal.setY(Lerp(Lerp(gy[0], gy[1], ix.f),
274 Lerp(gy[2], gy[3], ix.f), iz.f));
275 normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f),
276 Lerp(gz[2], gz[3], ix.f), iy.f));
277 normal.safeNormalize();
279 normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized();
282 const btScalar d0 = Lerp(Lerp(d[0], d[1], ix.f),
283 Lerp(d[3], d[2], ix.f), iy.f);
284 const btScalar d1 = Lerp(Lerp(d[4], d[5], ix.f),
285 Lerp(d[7], d[6], ix.f), iy.f);
286 return (Lerp(d0, d1, iz.f) - margin);
289 void BuildCell(Cell& c)
291 const btVector3 org = btVector3((btScalar)c.c[0],
295 for (int k = 0; k <= CELLSIZE; ++k)
297 const btScalar z = voxelsz * k + org.z();
298 for (int j = 0; j <= CELLSIZE; ++j)
300 const btScalar y = voxelsz * j + org.y();
301 for (int i = 0; i <= CELLSIZE; ++i)
303 const btScalar x = voxelsz * i + org.x();
304 c.d[i][j][k] = DistanceToShape(btVector3(x, y, z),
311 static inline btScalar DistanceToShape(const btVector3& x,
312 const btCollisionShape* shape)
316 if (shape->isConvex())
318 btGjkEpaSolver2::sResults res;
319 const btConvexShape* csh = static_cast<const btConvexShape*>(shape);
320 return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res));
325 static inline IntFrac Decompose(btScalar x)
327 /* That one need a lot of improvements... */
328 /* Remove test, faster floor... */
331 const int o = x < 0 ? (int)(-x + 1) : 0;
334 const btScalar k = (x - r.b) * CELLSIZE;
341 static inline btScalar Lerp(btScalar a, btScalar b, btScalar t)
343 return (a + (b - a) * t);
347 static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape)
356 //memset may be needed in case of additional (uninitialized) padding!
357 //memset(&myset, 0, sizeof(btS));
363 myset.p = (void*)shape;
364 const char* ptr = (const char*)&myset;
366 unsigned int result = HsiehHash(ptr, sizeof(btS));
372 #endif //BT_SPARSE_SDF_H