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;hash+=hash>>5;
38  hash^=hash<<4;hash+=hash>>17;
39  hash^=hash<<25;hash+=hash>>6;
40  return(hash);
41 }
42 
43 template <const int CELLSIZE>
44 struct btSparseSdf
45 {
46  //
47  // Inner types
48  //
49  struct IntFrac
50  {
51  int b;
52  int i;
53  btScalar f;
54  };
55  struct Cell
56  {
57  btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
58  int c[3];
59  int puid;
60  unsigned hash;
61  const btCollisionShape* pclient;
62  Cell* next;
63  };
64  //
65  // Fields
66  //
67 
68  btAlignedObjectArray<Cell*> cells;
69  btScalar voxelsz;
70  int puid;
71  int ncells;
72  int nprobes;
73  int nqueries;
74 
75  //
76  // Methods
77  //
78 
79  //
80  void Initialize(int hashsize=2383)
81  {
82  cells.resize(hashsize,0);
83  Reset();
84  }
85  //
86  void Reset()
87  {
88  for(int i=0,ni=cells.size();i<ni;++i)
89  {
90  Cell* pc=cells[i];
91  cells[i]=0;
92  while(pc)
93  {
94  Cell* pn=pc->next;
95  delete pc;
96  pc=pn;
97  }
98  }
99  voxelsz =0.25;
100  puid =0;
101  ncells =0;
102  nprobes =1;
103  nqueries =1;
104  }
105  //
106  void GarbageCollect(int lifetime=256)
107  {
108  const int life=puid-lifetime;
109  for(int i=0;i<cells.size();++i)
110  {
111  Cell*& root=cells[i];
112  Cell* pp=0;
113  Cell* pc=root;
114  while(pc)
115  {
116  Cell* pn=pc->next;
117  if(pc->puid<life)
118  {
119  if(pp) pp->next=pn; else root=pn;
120  delete pc;pc=pp;--ncells;
121  }
122  pp=pc;pc=pn;
123  }
124  }
125  //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
126  nqueries=1;
127  nprobes=1;
128  ++puid;
129  /* else setup a priority list... */
130  }
131  //
132  int RemoveReferences(btCollisionShape* pcs)
133  {
134  int refcount=0;
135  for(int i=0;i<cells.size();++i)
136  {
137  Cell*& root=cells[i];
138  Cell* pp=0;
139  Cell* pc=root;
140  while(pc)
141  {
142  Cell* pn=pc->next;
143  if(pc->pclient==pcs)
144  {
145  if(pp) pp->next=pn; else root=pn;
146  delete pc;pc=pp;++refcount;
147  }
148  pp=pc;pc=pn;
149  }
150  }
151  return(refcount);
152  }
153  //
154  btScalar Evaluate( const btVector3& x,
155  const btCollisionShape* shape,
156  btVector3& normal,
157  btScalar margin)
158  {
159  /* Lookup cell */
160  const btVector3 scx=x/voxelsz;
161  const IntFrac ix=Decompose(scx.x());
162  const IntFrac iy=Decompose(scx.y());
163  const IntFrac iz=Decompose(scx.z());
164  const unsigned h=Hash(ix.b,iy.b,iz.b,shape);
165  Cell*& root=cells[static_cast<int>(h%cells.size())];
166  Cell* c=root;
167  ++nqueries;
168  while(c)
169  {
170  ++nprobes;
171  if( (c->hash==h) &&
172  (c->c[0]==ix.b) &&
173  (c->c[1]==iy.b) &&
174  (c->c[2]==iz.b) &&
175  (c->pclient==shape))
176  { break; }
177  else
178  { c=c->next; }
179  }
180  if(!c)
181  {
182  ++nprobes;
183  ++ncells;
184  c=new Cell();
185  c->next=root;root=c;
186  c->pclient=shape;
187  c->hash=h;
188  c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
189  BuildCell(*c);
190  }
191  c->puid=puid;
192  /* Extract infos */
193  const int o[]={ ix.i,iy.i,iz.i};
194  const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0],
195  c->d[o[0]+1][o[1]+0][o[2]+0],
196  c->d[o[0]+1][o[1]+1][o[2]+0],
197  c->d[o[0]+0][o[1]+1][o[2]+0],
198  c->d[o[0]+0][o[1]+0][o[2]+1],
199  c->d[o[0]+1][o[1]+0][o[2]+1],
200  c->d[o[0]+1][o[1]+1][o[2]+1],
201  c->d[o[0]+0][o[1]+1][o[2]+1]};
202  /* Normal */
203 #if 1
204  const btScalar gx[]={ d[1]-d[0],d[2]-d[3],
205  d[5]-d[4],d[6]-d[7]};
206  const btScalar gy[]={ d[3]-d[0],d[2]-d[1],
207  d[7]-d[4],d[6]-d[5]};
208  const btScalar gz[]={ d[4]-d[0],d[5]-d[1],
209  d[7]-d[3],d[6]-d[2]};
210  normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f),
211  Lerp(gx[2],gx[3],iy.f),iz.f));
212  normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f),
213  Lerp(gy[2],gy[3],ix.f),iz.f));
214  normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f),
215  Lerp(gz[2],gz[3],ix.f),iy.f));
216  normal = normal.normalized();
217 #else
218  normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
219 #endif
220  /* Distance */
221  const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f),
222  Lerp(d[3],d[2],ix.f),iy.f);
223  const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f),
224  Lerp(d[7],d[6],ix.f),iy.f);
225  return(Lerp(d0,d1,iz.f)-margin);
226  }
227  //
228  void BuildCell(Cell& c)
229  {
230  const btVector3 org=btVector3( (btScalar)c.c[0],
231  (btScalar)c.c[1],
232  (btScalar)c.c[2]) *
233  CELLSIZE*voxelsz;
234  for(int k=0;k<=CELLSIZE;++k)
235  {
236  const btScalar z=voxelsz*k+org.z();
237  for(int j=0;j<=CELLSIZE;++j)
238  {
239  const btScalar y=voxelsz*j+org.y();
240  for(int i=0;i<=CELLSIZE;++i)
241  {
242  const btScalar x=voxelsz*i+org.x();
243  c.d[i][j][k]=DistanceToShape( btVector3(x,y,z),
244  c.pclient);
245  }
246  }
247  }
248  }
249  //
250  static inline btScalar DistanceToShape(const btVector3& x,
251  const btCollisionShape* shape)
252  {
253  btTransform unit;
254  unit.setIdentity();
255  if(shape->isConvex())
256  {
257  btGjkEpaSolver2::sResults res;
258  const btConvexShape* csh=static_cast<const btConvexShape*>(shape);
259  return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
260  }
261  return(0);
262  }
263  //
264  static inline IntFrac Decompose(btScalar x)
265  {
266  /* That one need a lot of improvements... */
267  /* Remove test, faster floor... */
268  IntFrac r;
269  x/=CELLSIZE;
270  const int o=x<0?(int)(-x+1):0;
271  x+=o;r.b=(int)x;
272  const btScalar k=(x-r.b)*CELLSIZE;
273  r.i=(int)k;r.f=k-r.i;r.b-=o;
274  return(r);
275  }
276  //
277  static inline btScalar Lerp(btScalar a,btScalar b,btScalar t)
278  {
279  return(a+(b-a)*t);
280  }
281 
282 
283 
284  //
285  static inline unsigned int Hash(int x,int y,int z,const btCollisionShape* shape)
286  {
287  struct btS
288  {
289  int x,y,z;
290  void* p;
291  };
292 
293  btS myset;
294 
295  myset.x=x;myset.y=y;myset.z=z;myset.p=(void*)shape;
296  const void* ptr = &myset;
297 
298  unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
299 
300 
301  return result;
302  }
303 };
304 
305 
306 #endif //BT_SPARSE_SDF_H
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4