00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00016
00017 #ifndef BT_SPARSE_SDF_H
00018 #define BT_SPARSE_SDF_H
00019
00020 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
00022
00023
00024 template <const int DWORDLEN>
00025 unsigned int HsiehHash(const void* pdata)
00026 {
00027 const unsigned short* data=(const unsigned short*)pdata;
00028 unsigned hash=DWORDLEN<<2,tmp;
00029 for(int i=0;i<DWORDLEN;++i)
00030 {
00031 hash += data[0];
00032 tmp = (data[1]<<11)^hash;
00033 hash = (hash<<16)^tmp;
00034 data += 2;
00035 hash += hash>>11;
00036 }
00037 hash^=hash<<3;hash+=hash>>5;
00038 hash^=hash<<4;hash+=hash>>17;
00039 hash^=hash<<25;hash+=hash>>6;
00040 return(hash);
00041 }
00042
00043 template <const int CELLSIZE>
00044 struct btSparseSdf
00045 {
00046
00047
00048
00049 struct IntFrac
00050 {
00051 int b;
00052 int i;
00053 btScalar f;
00054 };
00055 struct Cell
00056 {
00057 btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
00058 int c[3];
00059 int puid;
00060 unsigned hash;
00061 btCollisionShape* pclient;
00062 Cell* next;
00063 };
00064
00065
00066
00067
00068 btAlignedObjectArray<Cell*> cells;
00069 btScalar voxelsz;
00070 int puid;
00071 int ncells;
00072 int nprobes;
00073 int nqueries;
00074
00075
00076
00077
00078
00079
00080 void Initialize(int hashsize=2383)
00081 {
00082 cells.resize(hashsize,0);
00083 Reset();
00084 }
00085
00086 void Reset()
00087 {
00088 for(int i=0,ni=cells.size();i<ni;++i)
00089 {
00090 Cell* pc=cells[i];
00091 cells[i]=0;
00092 while(pc)
00093 {
00094 Cell* pn=pc->next;
00095 delete pc;
00096 pc=pn;
00097 }
00098 }
00099 voxelsz =0.25;
00100 puid =0;
00101 ncells =0;
00102 nprobes =1;
00103 nqueries =1;
00104 }
00105
00106 void GarbageCollect(int lifetime=256)
00107 {
00108 const int life=puid-lifetime;
00109 for(int i=0;i<cells.size();++i)
00110 {
00111 Cell*& root=cells[i];
00112 Cell* pp=0;
00113 Cell* pc=root;
00114 while(pc)
00115 {
00116 Cell* pn=pc->next;
00117 if(pc->puid<life)
00118 {
00119 if(pp) pp->next=pn; else root=pn;
00120 delete pc;pc=pp;--ncells;
00121 }
00122 pp=pc;pc=pn;
00123 }
00124 }
00125
00126 nqueries=1;
00127 nprobes=1;
00128 ++puid;
00129
00130 }
00131
00132 int RemoveReferences(btCollisionShape* pcs)
00133 {
00134 int refcount=0;
00135 for(int i=0;i<cells.size();++i)
00136 {
00137 Cell*& root=cells[i];
00138 Cell* pp=0;
00139 Cell* pc=root;
00140 while(pc)
00141 {
00142 Cell* pn=pc->next;
00143 if(pc->pclient==pcs)
00144 {
00145 if(pp) pp->next=pn; else root=pn;
00146 delete pc;pc=pp;++refcount;
00147 }
00148 pp=pc;pc=pn;
00149 }
00150 }
00151 return(refcount);
00152 }
00153
00154 btScalar Evaluate( const btVector3& x,
00155 btCollisionShape* shape,
00156 btVector3& normal,
00157 btScalar margin)
00158 {
00159
00160 const btVector3 scx=x/voxelsz;
00161 const IntFrac ix=Decompose(scx.x());
00162 const IntFrac iy=Decompose(scx.y());
00163 const IntFrac iz=Decompose(scx.z());
00164 const unsigned h=Hash(ix.b,iy.b,iz.b,shape);
00165 Cell*& root=cells[static_cast<int>(h%cells.size())];
00166 Cell* c=root;
00167 ++nqueries;
00168 while(c)
00169 {
00170 ++nprobes;
00171 if( (c->hash==h) &&
00172 (c->c[0]==ix.b) &&
00173 (c->c[1]==iy.b) &&
00174 (c->c[2]==iz.b) &&
00175 (c->pclient==shape))
00176 { break; }
00177 else
00178 { c=c->next; }
00179 }
00180 if(!c)
00181 {
00182 ++nprobes;
00183 ++ncells;
00184 c=new Cell();
00185 c->next=root;root=c;
00186 c->pclient=shape;
00187 c->hash=h;
00188 c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
00189 BuildCell(*c);
00190 }
00191 c->puid=puid;
00192
00193 const int o[]={ ix.i,iy.i,iz.i};
00194 const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0],
00195 c->d[o[0]+1][o[1]+0][o[2]+0],
00196 c->d[o[0]+1][o[1]+1][o[2]+0],
00197 c->d[o[0]+0][o[1]+1][o[2]+0],
00198 c->d[o[0]+0][o[1]+0][o[2]+1],
00199 c->d[o[0]+1][o[1]+0][o[2]+1],
00200 c->d[o[0]+1][o[1]+1][o[2]+1],
00201 c->d[o[0]+0][o[1]+1][o[2]+1]};
00202
00203 #if 1
00204 const btScalar gx[]={ d[1]-d[0],d[2]-d[3],
00205 d[5]-d[4],d[6]-d[7]};
00206 const btScalar gy[]={ d[3]-d[0],d[2]-d[1],
00207 d[7]-d[4],d[6]-d[5]};
00208 const btScalar gz[]={ d[4]-d[0],d[5]-d[1],
00209 d[7]-d[3],d[6]-d[2]};
00210 normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f),
00211 Lerp(gx[2],gx[3],iy.f),iz.f));
00212 normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f),
00213 Lerp(gy[2],gy[3],ix.f),iz.f));
00214 normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f),
00215 Lerp(gz[2],gz[3],ix.f),iy.f));
00216 normal = normal.normalized();
00217 #else
00218 normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
00219 #endif
00220
00221 const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f),
00222 Lerp(d[3],d[2],ix.f),iy.f);
00223 const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f),
00224 Lerp(d[7],d[6],ix.f),iy.f);
00225 return(Lerp(d0,d1,iz.f)-margin);
00226 }
00227
00228 void BuildCell(Cell& c)
00229 {
00230 const btVector3 org=btVector3( (btScalar)c.c[0],
00231 (btScalar)c.c[1],
00232 (btScalar)c.c[2]) *
00233 CELLSIZE*voxelsz;
00234 for(int k=0;k<=CELLSIZE;++k)
00235 {
00236 const btScalar z=voxelsz*k+org.z();
00237 for(int j=0;j<=CELLSIZE;++j)
00238 {
00239 const btScalar y=voxelsz*j+org.y();
00240 for(int i=0;i<=CELLSIZE;++i)
00241 {
00242 const btScalar x=voxelsz*i+org.x();
00243 c.d[i][j][k]=DistanceToShape( btVector3(x,y,z),
00244 c.pclient);
00245 }
00246 }
00247 }
00248 }
00249
00250 static inline btScalar DistanceToShape(const btVector3& x,
00251 btCollisionShape* shape)
00252 {
00253 btTransform unit;
00254 unit.setIdentity();
00255 if(shape->isConvex())
00256 {
00257 btGjkEpaSolver2::sResults res;
00258 btConvexShape* csh=static_cast<btConvexShape*>(shape);
00259 return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
00260 }
00261 return(0);
00262 }
00263
00264 static inline IntFrac Decompose(btScalar x)
00265 {
00266
00267
00268 IntFrac r;
00269 x/=CELLSIZE;
00270 const int o=x<0?(int)(-x+1):0;
00271 x+=o;r.b=(int)x;
00272 const btScalar k=(x-r.b)*CELLSIZE;
00273 r.i=(int)k;r.f=k-r.i;r.b-=o;
00274 return(r);
00275 }
00276
00277 static inline btScalar Lerp(btScalar a,btScalar b,btScalar t)
00278 {
00279 return(a+(b-a)*t);
00280 }
00281
00282
00283
00284
00285 static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape)
00286 {
00287 struct btS
00288 {
00289 int x,y,z;
00290 void* p;
00291 };
00292
00293 btS myset;
00294
00295 myset.x=x;myset.y=y;myset.z=z;myset.p=shape;
00296 const void* ptr = &myset;
00297
00298 unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
00299
00300
00301 return result;
00302 }
00303 };
00304
00305
00306 #endif //BT_SPARSE_SDF_H
