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

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