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
btSparseSdf::cells
btAlignedObjectArray< Cell * > cells
Definition: btSparseSDF.h:71
btGjkEpa2.h
btSparseSdf::IntFrac::b
int b
Definition: btSparseSDF.h:54
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btSparseSdf::DistanceToShape
static btScalar DistanceToShape(const btVector3 &x, const btCollisionShape *shape)
Definition: btSparseSDF.h:285
btSparseSdf::Evaluate
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:173
btSparseSdf::IntFrac
Definition: btSparseSDF.h:52
btSparseSdf
Definition: btSparseSDF.h:47
btSparseSdf::Cell::hash
unsigned hash
Definition: btSparseSDF.h:63
btGjkEpaSolver2::sResults
Definition: btGjkEpa2.h:33
btSparseSdf::Decompose
static IntFrac Decompose(btScalar x)
Definition: btSparseSDF.h:299
btSparseSdf::ncells
int ncells
Definition: btSparseSDF.h:74
btSparseSdf::voxelsz
btScalar voxelsz
Definition: btSparseSDF.h:72
HsiehHash
unsigned int HsiehHash(const void *pdata)
btSparseSdf implementation by Nathanael Presson
Definition: btSparseSDF.h:25
btSparseSdf::Cell::next
Cell * next
Definition: btSparseSDF.h:65
btGjkEpaSolver2::SignedDistance
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btSparseSdf::Cell
Definition: btSparseSDF.h:58
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btSparseSdf::Hash
static unsigned int Hash(int x, int y, int z, const btCollisionShape *shape)
Definition: btSparseSDF.h:321
btSparseSdf::Cell::pclient
const btCollisionShape * pclient
Definition: btSparseSDF.h:64
btSparseSdf::Lerp
static btScalar Lerp(btScalar a, btScalar b, btScalar t)
Definition: btSparseSDF.h:315
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
Definition: btCollisionShape.h:26
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
btSparseSdf::RemoveReferences
int RemoveReferences(btCollisionShape *pcs)
Definition: btSparseSDF.h:145
btSparseSdf::BuildCell
void BuildCell(Cell &c)
Definition: btSparseSDF.h:263
btSparseSdf::puid
int puid
Definition: btSparseSDF.h:73
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btSparseSdf::Initialize
void Initialize(int hashsize=2383, int clampCells=256 *1024)
Definition: btSparseSDF.h:84
btSparseSdf::Reset
void Reset()
Definition: btSparseSDF.h:93
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btSparseSdf::IntFrac::i
int i
Definition: btSparseSDF.h:55
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:52
btCollisionShape::isConvex
bool isConvex() const
Definition: btCollisionShape.h:69
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
btSparseSdf::GarbageCollect
void GarbageCollect(int lifetime=256)
Definition: btSparseSDF.h:113
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btSparseSdf::IntFrac::f
btScalar f
Definition: btSparseSDF.h:56
btSparseSdf::nprobes
int nprobes
Definition: btSparseSDF.h:76
btSparseSdf::Cell::d
btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]
Definition: btSparseSDF.h:60
btSparseSdf::Cell::puid
int puid
Definition: btSparseSDF.h:62
btCollisionObject.h
btSparseSdf::m_clampCells
int m_clampCells
Definition: btSparseSDF.h:75
btSparseSdf::nqueries
int nqueries
Definition: btSparseSDF.h:77
btSparseSdf::Cell::c
int c[3]
Definition: btSparseSDF.h:61
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579