Bullet Collision Detection & Physics Library: btSparseSDF.h Source File

Bullet Collision Detection & Physics Library

btSparseSDF.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
20#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
21#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
22
23// Fast Hash
24
25#if !defined(get16bits)
26#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
27#endif
28//
29// super hash function by Paul Hsieh
30//
31 inline unsigned int HsiehHash(const char* data, int len)
32{
33 unsigned int hash = len, tmp;
34 len >>= 2;
35
36 /* Main loop */
37 for (; len > 0; len--)
38 {
39 hash += get16bits(data);
40 tmp = (get16bits(data + 2) << 11) ^ hash;
41 hash = (hash << 16) ^ tmp;
42 data += 2 * sizeof(unsigned short);
43 hash += hash >> 11;
44 }
45
46 /* Force "avalanching" of final 127 bits */
47 hash ^= hash << 3;
48 hash += hash >> 5;
49 hash ^= hash << 4;
50 hash += hash >> 17;
51 hash ^= hash << 25;
52 hash += hash >> 6;
53
54 return hash;
55}
56
57template <const int CELLSIZE>
58 struct btSparseSdf
59{
60 //
61 // Inner types
62 //
63 struct IntFrac
64 {
65 int b;
66 int i;
67 btScalar f;
68 };
69 struct Cell
70 {
71 btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1];
72 int c[3];
73 int puid;
74 unsigned hash;
75 const btCollisionShape* pclient;
76 Cell* next;
77 };
78 //
79 // Fields
80 //
81
82 btAlignedObjectArray<Cell*> cells;
83 btScalar voxelsz;
84 btScalar m_defaultVoxelsz;
85 int puid;
86 int ncells;
87 int m_clampCells;
88 int nprobes;
89 int nqueries;
90
91 ~btSparseSdf()
92 {
93 Reset();
94 }
95 //
96 // Methods
97 //
98
99 //
100 void Initialize(int hashsize = 2383, int clampCells = 256 * 1024)
101 {
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;
107 Reset();
108 }
109 //
110
111 void setDefaultVoxelsz(btScalar sz)
112 {
113 m_defaultVoxelsz = sz;
114 }
115
116 void Reset()
117 {
118 for (int i = 0, ni = cells.size(); i < ni; ++i)
119 {
120 Cell* pc = cells[i];
121 cells[i] = 0;
122 while (pc)
123 {
124 Cell* pn = pc->next;
125 delete pc;
126 pc = pn;
127 }
128 }
129 voxelsz = m_defaultVoxelsz;
130 puid = 0;
131 ncells = 0;
132 nprobes = 1;
133 nqueries = 1;
134 }
135 //
136 void GarbageCollect(int lifetime = 256)
137 {
138 const int life = puid - lifetime;
139 for (int i = 0; i < cells.size(); ++i)
140 {
141 Cell*& root = cells[i];
142 Cell* pp = 0;
143 Cell* pc = root;
144 while (pc)
145 {
146 Cell* pn = pc->next;
147 if (pc->puid < life)
148 {
149 if (pp)
150 pp->next = pn;
151 else
152 root = pn;
153 delete pc;
154 pc = pp;
155 --ncells;
156 }
157 pp = pc;
158 pc = pn;
159 }
160 }
161 //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
162 nqueries = 1;
163 nprobes = 1;
164 ++puid;
165 /* else setup a priority list... */
166 }
167 //
168 int RemoveReferences(btCollisionShape* pcs)
169 {
170 int refcount = 0;
171 for (int i = 0; i < cells.size(); ++i)
172 {
173 Cell*& root = cells[i];
174 Cell* pp = 0;
175 Cell* pc = root;
176 while (pc)
177 {
178 Cell* pn = pc->next;
179 if (pc->pclient == pcs)
180 {
181 if (pp)
182 pp->next = pn;
183 else
184 root = pn;
185 delete pc;
186 pc = pp;
187 ++refcount;
188 }
189 pp = pc;
190 pc = pn;
191 }
192 }
193 return (refcount);
194 }
195 //
196 btScalar Evaluate(const btVector3& x,
197 const btCollisionShape* shape,
198 btVector3& normal,
199 btScalar margin)
200 {
201 /* Lookup cell */
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())];
208 Cell* c = root;
209 ++nqueries;
210 while (c)
211 {
212 ++nprobes;
213 if ((c->hash == h) &&
214 (c->c[0] == ix.b) &&
215 (c->c[1] == iy.b) &&
216 (c->c[2] == iz.b) &&
217 (c->pclient == shape))
218 {
219 break;
220 }
221 else
222 {
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);
225
226 c = c->next;
227 }
228 }
229 if (!c)
230 {
231 ++nprobes;
232 ++ncells;
233 //int sz = sizeof(Cell);
234 if (ncells > m_clampCells)
235 {
236 static int numResets = 0;
237 numResets++;
238 // printf("numResets=%d\n",numResets);
239 Reset();
240 }
241
242 c = new Cell();
243 c->next = root;
244 root = c;
245 c->pclient = shape;
246 c->hash = h;
247 c->c[0] = ix.b;
248 c->c[1] = iy.b;
249 c->c[2] = iz.b;
250 BuildCell(*c);
251 }
252 c->puid = puid;
253 /* Extract infos */
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]};
263 /* Normal */
264#if 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();
278#else
279 normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized();
280#endif
281 /* Distance */
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);
287 }
288 //
289 void BuildCell(Cell& c)
290 {
291 const btVector3 org = btVector3((btScalar)c.c[0],
292 (btScalar)c.c[1],
293 (btScalar)c.c[2]) *
294 CELLSIZE * voxelsz;
295 for (int k = 0; k <= CELLSIZE; ++k)
296 {
297 const btScalar z = voxelsz * k + org.z();
298 for (int j = 0; j <= CELLSIZE; ++j)
299 {
300 const btScalar y = voxelsz * j + org.y();
301 for (int i = 0; i <= CELLSIZE; ++i)
302 {
303 const btScalar x = voxelsz * i + org.x();
304 c.d[i][j][k] = DistanceToShape(btVector3(x, y, z),
305 c.pclient);
306 }
307 }
308 }
309 }
310 //
311 static inline btScalar DistanceToShape(const btVector3& x,
312 const btCollisionShape* shape)
313 {
314 btTransform unit;
315 unit.setIdentity();
316 if (shape->isConvex())
317 {
318 btGjkEpaSolver2::sResults res;
319 const btConvexShape* csh = static_cast<const btConvexShape*>(shape);
320 return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res));
321 }
322 return (0);
323 }
324 //
325 static inline IntFrac Decompose(btScalar x)
326 {
327 /* That one need a lot of improvements... */
328 /* Remove test, faster floor... */
329 IntFrac r;
330 x /= CELLSIZE;
331 const int o = x < 0 ? (int)(-x + 1) : 0;
332 x += o;
333 r.b = (int)x;
334 const btScalar k = (x - r.b) * CELLSIZE;
335 r.i = (int)k;
336 r.f = k - r.i;
337 r.b -= o;
338 return (r);
339 }
340 //
341 static inline btScalar Lerp(btScalar a, btScalar b, btScalar t)
342 {
343 return (a + (b - a) * t);
344 }
345
346 //
347 static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape)
348 {
349 struct btS
350 {
351 int x, y, z, w;
352 void* p;
353 };
354
355 btS myset;
356 //memset may be needed in case of additional (uninitialized) padding!
357 //memset(&myset, 0, sizeof(btS));
358
359 myset.x = x;
360 myset.y = y;
361 myset.z = z;
362 myset.w = 0;
363 myset.p = (void*)shape;
364 const char* ptr = (const char*)&myset;
365
366 unsigned int result = HsiehHash(ptr, sizeof(btS));
367
368 return result;
369 }
370};
371
372#endif //BT_SPARSE_SDF_H
btCollisionObject.h
btGjkEpa2.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
HsiehHash
unsigned int HsiehHash(const char *data, int len)
Definition: btSparseSDF.h:31
get16bits
#define get16bits(d)
btSparseSdf implementation by Nathanael Presson
Definition: btSparseSDF.h:26
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:46
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
Definition: btCollisionShape.h:28
btCollisionShape::isConvex
bool isConvex() const
Definition: btCollisionShape.h:70
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:167
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3::safeNormalize
btVector3 & safeNormalize()
Definition: btVector3.h:286
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btGjkEpaSolver2::sResults
Definition: btGjkEpa2.h:34
btGjkEpaSolver2::SignedDistance
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
btSparseSdf::Cell
Definition: btSparseSDF.h:70
btSparseSdf::Cell::next
Cell * next
Definition: btSparseSDF.h:76
btSparseSdf::Cell::pclient
const btCollisionShape * pclient
Definition: btSparseSDF.h:75
btSparseSdf::Cell::c
int c[3]
Definition: btSparseSDF.h:72
btSparseSdf::Cell::hash
unsigned hash
Definition: btSparseSDF.h:74
btSparseSdf::Cell::d
btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]
Definition: btSparseSDF.h:71
btSparseSdf::Cell::puid
int puid
Definition: btSparseSDF.h:73
btSparseSdf::IntFrac
Definition: btSparseSDF.h:64
btSparseSdf::IntFrac::b
int b
Definition: btSparseSDF.h:65
btSparseSdf::IntFrac::i
int i
Definition: btSparseSDF.h:66
btSparseSdf::IntFrac::f
btScalar f
Definition: btSparseSDF.h:67
btSparseSdf
Definition: btSparseSDF.h:59
btSparseSdf::ncells
int ncells
Definition: btSparseSDF.h:86
btSparseSdf::GarbageCollect
void GarbageCollect(int lifetime=256)
Definition: btSparseSDF.h:136
btSparseSdf::Decompose
static IntFrac Decompose(btScalar x)
Definition: btSparseSDF.h:325
btSparseSdf::Reset
void Reset()
Definition: btSparseSDF.h:116
btSparseSdf::~btSparseSdf
~btSparseSdf()
Definition: btSparseSDF.h:91
btSparseSdf::DistanceToShape
static btScalar DistanceToShape(const btVector3 &x, const btCollisionShape *shape)
Definition: btSparseSDF.h:311
btSparseSdf::RemoveReferences
int RemoveReferences(btCollisionShape *pcs)
Definition: btSparseSDF.h:168
btSparseSdf::m_clampCells
int m_clampCells
Definition: btSparseSDF.h:87
btSparseSdf::m_defaultVoxelsz
btScalar m_defaultVoxelsz
Definition: btSparseSDF.h:84
btSparseSdf::nprobes
int nprobes
Definition: btSparseSDF.h:88
btSparseSdf::cells
btAlignedObjectArray< Cell * > cells
Definition: btSparseSDF.h:82
btSparseSdf::Evaluate
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:196
btSparseSdf::nqueries
int nqueries
Definition: btSparseSDF.h:89
btSparseSdf::BuildCell
void BuildCell(Cell &c)
Definition: btSparseSDF.h:289
btSparseSdf::Lerp
static btScalar Lerp(btScalar a, btScalar b, btScalar t)
Definition: btSparseSDF.h:341
btSparseSdf::Hash
static unsigned int Hash(int x, int y, int z, const btCollisionShape *shape)
Definition: btSparseSDF.h:347
btSparseSdf::setDefaultVoxelsz
void setDefaultVoxelsz(btScalar sz)
Definition: btSparseSDF.h:111
btSparseSdf::voxelsz
btScalar voxelsz
Definition: btSparseSDF.h:83
btSparseSdf::Initialize
void Initialize(int hashsize=2383, int clampCells=256 *1024)
Definition: btSparseSDF.h:100
btSparseSdf::puid
int puid
Definition: btSparseSDF.h:85