Bullet Collision Detection & Physics Library
btSparseSDF.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
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:
10 
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.
14 */
16 
17 #ifndef BT_SPARSE_SDF_H
18 #define BT_SPARSE_SDF_H
19 
22 
23 // Modified Paul Hsieh hash
24 template <const int DWORDLEN>
25 unsigned int HsiehHash(const void* pdata)
26 {
27  const unsigned short* data = (const unsigned short*)pdata;
28  unsigned hash = DWORDLEN << 2, tmp;
29  for (int i = 0; i < DWORDLEN; ++i)
30  {
31  hash += data[0];
32  tmp = (data[1] << 11) ^ hash;
33  hash = (hash << 16) ^ tmp;
34  data += 2;
35  hash += hash >> 11;
36  }
37  hash ^= hash << 3;
38  hash += hash >> 5;
39  hash ^= hash << 4;
40  hash += hash >> 17;
41  hash ^= hash << 25;
42  hash += hash >> 6;
43  return (hash);
44 }
45 
46 template <const int CELLSIZE>
48 {
49  //
50  // Inner types
51  //
52  struct IntFrac
53  {
54  int b;
55  int i;
57  };
58  struct Cell
59  {
60  btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1];
61  int c[3];
62  int puid;
63  unsigned hash;
66  };
67  //
68  // Fields
69  //
70 
73  int puid;
74  int ncells;
76  int nprobes;
77  int nqueries;
78 
79  //
80  // Methods
81  //
82 
83  //
84  void Initialize(int hashsize = 2383, int clampCells = 256 * 1024)
85  {
86  //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated
87  //if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
88  m_clampCells = clampCells;
89  cells.resize(hashsize, 0);
90  Reset();
91  }
92  //
93  void Reset()
94  {
95  for (int i = 0, ni = cells.size(); i < ni; ++i)
96  {
97  Cell* pc = cells[i];
98  cells[i] = 0;
99  while (pc)
100  {
101  Cell* pn = pc->next;
102  delete pc;
103  pc = pn;
104  }
105  }
106  voxelsz = 0.25;
107  puid = 0;
108  ncells = 0;
109  nprobes = 1;
110  nqueries = 1;
111  }
112  //
113  void GarbageCollect(int lifetime = 256)
114  {
115  const int life = puid - lifetime;
116  for (int i = 0; i < cells.size(); ++i)
117  {
118  Cell*& root = cells[i];
119  Cell* pp = 0;
120  Cell* pc = root;
121  while (pc)
122  {
123  Cell* pn = pc->next;
124  if (pc->puid < life)
125  {
126  if (pp)
127  pp->next = pn;
128  else
129  root = pn;
130  delete pc;
131  pc = pp;
132  --ncells;
133  }
134  pp = pc;
135  pc = pn;
136  }
137  }
138  //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
139  nqueries = 1;
140  nprobes = 1;
141  ++puid;
142  /* else setup a priority list... */
143  }
144  //
146  {
147  int refcount = 0;
148  for (int i = 0; i < cells.size(); ++i)
149  {
150  Cell*& root = cells[i];
151  Cell* pp = 0;
152  Cell* pc = root;
153  while (pc)
154  {
155  Cell* pn = pc->next;
156  if (pc->pclient == pcs)
157  {
158  if (pp)
159  pp->next = pn;
160  else
161  root = pn;
162  delete pc;
163  pc = pp;
164  ++refcount;
165  }
166  pp = pc;
167  pc = pn;
168  }
169  }
170  return (refcount);
171  }
172  //
174  const btCollisionShape* shape,
175  btVector3& normal,
176  btScalar margin)
177  {
178  /* Lookup cell */
179  const btVector3 scx = x / voxelsz;
180  const IntFrac ix = Decompose(scx.x());
181  const IntFrac iy = Decompose(scx.y());
182  const IntFrac iz = Decompose(scx.z());
183  const unsigned h = Hash(ix.b, iy.b, iz.b, shape);
184  Cell*& root = cells[static_cast<int>(h % cells.size())];
185  Cell* c = root;
186  ++nqueries;
187  while (c)
188  {
189  ++nprobes;
190  if ((c->hash == h) &&
191  (c->c[0] == ix.b) &&
192  (c->c[1] == iy.b) &&
193  (c->c[2] == iz.b) &&
194  (c->pclient == shape))
195  {
196  break;
197  }
198  else
199  {
200  c = c->next;
201  }
202  }
203  if (!c)
204  {
205  ++nprobes;
206  ++ncells;
207  //int sz = sizeof(Cell);
208  if (ncells > m_clampCells)
209  {
210  static int numResets = 0;
211  numResets++;
212  // printf("numResets=%d\n",numResets);
213  Reset();
214  }
215 
216  c = new Cell();
217  c->next = root;
218  root = c;
219  c->pclient = shape;
220  c->hash = h;
221  c->c[0] = ix.b;
222  c->c[1] = iy.b;
223  c->c[2] = iz.b;
224  BuildCell(*c);
225  }
226  c->puid = puid;
227  /* Extract infos */
228  const int o[] = {ix.i, iy.i, iz.i};
229  const btScalar d[] = {c->d[o[0] + 0][o[1] + 0][o[2] + 0],
230  c->d[o[0] + 1][o[1] + 0][o[2] + 0],
231  c->d[o[0] + 1][o[1] + 1][o[2] + 0],
232  c->d[o[0] + 0][o[1] + 1][o[2] + 0],
233  c->d[o[0] + 0][o[1] + 0][o[2] + 1],
234  c->d[o[0] + 1][o[1] + 0][o[2] + 1],
235  c->d[o[0] + 1][o[1] + 1][o[2] + 1],
236  c->d[o[0] + 0][o[1] + 1][o[2] + 1]};
237  /* Normal */
238 #if 1
239  const btScalar gx[] = {d[1] - d[0], d[2] - d[3],
240  d[5] - d[4], d[6] - d[7]};
241  const btScalar gy[] = {d[3] - d[0], d[2] - d[1],
242  d[7] - d[4], d[6] - d[5]};
243  const btScalar gz[] = {d[4] - d[0], d[5] - d[1],
244  d[7] - d[3], d[6] - d[2]};
245  normal.setX(Lerp(Lerp(gx[0], gx[1], iy.f),
246  Lerp(gx[2], gx[3], iy.f), iz.f));
247  normal.setY(Lerp(Lerp(gy[0], gy[1], ix.f),
248  Lerp(gy[2], gy[3], ix.f), iz.f));
249  normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f),
250  Lerp(gz[2], gz[3], ix.f), iy.f));
251  normal = normal.normalized();
252 #else
253  normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized();
254 #endif
255  /* Distance */
256  const btScalar d0 = Lerp(Lerp(d[0], d[1], ix.f),
257  Lerp(d[3], d[2], ix.f), iy.f);
258  const btScalar d1 = Lerp(Lerp(d[4], d[5], ix.f),
259  Lerp(d[7], d[6], ix.f), iy.f);
260  return (Lerp(d0, d1, iz.f) - margin);
261  }
262  //
263  void BuildCell(Cell& c)
264  {
265  const btVector3 org = btVector3((btScalar)c.c[0],
266  (btScalar)c.c[1],
267  (btScalar)c.c[2]) *
268  CELLSIZE * voxelsz;
269  for (int k = 0; k <= CELLSIZE; ++k)
270  {
271  const btScalar z = voxelsz * k + org.z();
272  for (int j = 0; j <= CELLSIZE; ++j)
273  {
274  const btScalar y = voxelsz * j + org.y();
275  for (int i = 0; i <= CELLSIZE; ++i)
276  {
277  const btScalar x = voxelsz * i + org.x();
278  c.d[i][j][k] = DistanceToShape(btVector3(x, y, z),
279  c.pclient);
280  }
281  }
282  }
283  }
284  //
285  static inline btScalar DistanceToShape(const btVector3& x,
286  const btCollisionShape* shape)
287  {
288  btTransform unit;
289  unit.setIdentity();
290  if (shape->isConvex())
291  {
293  const btConvexShape* csh = static_cast<const btConvexShape*>(shape);
294  return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res));
295  }
296  return (0);
297  }
298  //
299  static inline IntFrac Decompose(btScalar x)
300  {
301  /* That one need a lot of improvements... */
302  /* Remove test, faster floor... */
303  IntFrac r;
304  x /= CELLSIZE;
305  const int o = x < 0 ? (int)(-x + 1) : 0;
306  x += o;
307  r.b = (int)x;
308  const btScalar k = (x - r.b) * CELLSIZE;
309  r.i = (int)k;
310  r.f = k - r.i;
311  r.b -= o;
312  return (r);
313  }
314  //
315  static inline btScalar Lerp(btScalar a, btScalar b, btScalar t)
316  {
317  return (a + (b - a) * t);
318  }
319 
320  //
321  static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape)
322  {
323  struct btS
324  {
325  int x, y, z;
326  void* p;
327  };
328 
329  btS myset;
330 
331  myset.x = x;
332  myset.y = y;
333  myset.z = z;
334  myset.p = (void*)shape;
335  const void* ptr = &myset;
336 
337  unsigned int result = HsiehHash<sizeof(btS) / 4>(ptr);
338 
339  return result;
340  }
341 };
342 
343 #endif //BT_SPARSE_SDF_H
void GarbageCollect(int lifetime=256)
Definition: btSparseSDF.h:113
void BuildCell(Cell &c)
Definition: btSparseSDF.h:263
bool isConvex() const
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
unsigned int HsiehHash(const void *pdata)
btSparseSdf implementation by Nathanael Presson
Definition: btSparseSDF.h:25
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
btAlignedObjectArray< Cell * > cells
Definition: btSparseSDF.h:71
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
int RemoveReferences(btCollisionShape *pcs)
Definition: btSparseSDF.h:145
static btScalar Lerp(btScalar a, btScalar b, btScalar t)
Definition: btSparseSDF.h:315
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]
Definition: btSparseSDF.h:60
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:173
static unsigned int Hash(int x, int y, int z, const btCollisionShape *shape)
Definition: btSparseSDF.h:321
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
const btCollisionShape * pclient
Definition: btSparseSDF.h:64
btScalar voxelsz
Definition: btSparseSDF.h:72
static IntFrac Decompose(btScalar x)
Definition: btSparseSDF.h:299
int m_clampCells
Definition: btSparseSDF.h:75
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
static btScalar DistanceToShape(const btVector3 &x, const btCollisionShape *shape)
Definition: btSparseSDF.h:285
void Reset()
Definition: btSparseSDF.h:93
void Initialize(int hashsize=2383, int clampCells=256 *1024)
Definition: btSparseSDF.h:84