| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| ///btSparseSdf implementation by Nathanael Presson |
| |
| #ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ |
| #define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ |
| |
| #include "BulletCollision/CollisionDispatch/btCollisionObject.h" |
| #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" |
| |
| // Modified Paul Hsieh hash |
| template <const int DWORDLEN> |
| unsigned int HsiehHash(const void* pdata) |
| { |
| const unsigned short* data=(const unsigned short*)pdata; |
| unsigned hash=DWORDLEN<<2,tmp; |
| for(int i=0;i<DWORDLEN;++i) |
| { |
| hash += data[0]; |
| tmp = (data[1]<<11)^hash; |
| hash = (hash<<16)^tmp; |
| data += 2; |
| hash += hash>>11; |
| } |
| hash^=hash<<3;hash+=hash>>5; |
| hash^=hash<<4;hash+=hash>>17; |
| hash^=hash<<25;hash+=hash>>6; |
| return(hash); |
| } |
| |
| template <const int CELLSIZE> |
| struct btSparseSdf |
| { |
| // |
| // Inner types |
| // |
| struct IntFrac |
| { |
| int b; |
| int i; |
| btScalar f; |
| }; |
| struct Cell |
| { |
| btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; |
| int c[3]; |
| int puid; |
| unsigned hash; |
| btCollisionShape* pclient; |
| Cell* next; |
| }; |
| // |
| // Fields |
| // |
| |
| btAlignedObjectArray<Cell*> cells; |
| btScalar voxelsz; |
| int puid; |
| int ncells; |
| int nprobes; |
| int nqueries; |
| |
| // |
| // Methods |
| // |
| |
| // |
| void Initialize(int hashsize=2383) |
| { |
| cells.resize(hashsize,0); |
| Reset(); |
| } |
| // |
| void Reset() |
| { |
| for(int i=0,ni=cells.size();i<ni;++i) |
| { |
| Cell* pc=cells[i]; |
| cells[i]=0; |
| while(pc) |
| { |
| Cell* pn=pc->next; |
| delete pc; |
| pc=pn; |
| } |
| } |
| voxelsz =0.25; |
| puid =0; |
| ncells =0; |
| nprobes =1; |
| nqueries =1; |
| } |
| // |
| void GarbageCollect(int lifetime=256) |
| { |
| const int life=puid-lifetime; |
| for(int i=0;i<cells.size();++i) |
| { |
| Cell*& root=cells[i]; |
| Cell* pp=0; |
| Cell* pc=root; |
| while(pc) |
| { |
| Cell* pn=pc->next; |
| if(pc->puid<life) |
| { |
| if(pp) pp->next=pn; else root=pn; |
| delete pc;pc=pp;--ncells; |
| } |
| pp=pc;pc=pn; |
| } |
| } |
| //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); |
| nqueries=1; |
| nprobes=1; |
| ++puid; ///@todo: Reset puid's when int range limit is reached */ |
| /* else setup a priority list... */ |
| } |
| // |
| int RemoveReferences(btCollisionShape* pcs) |
| { |
| int refcount=0; |
| for(int i=0;i<cells.size();++i) |
| { |
| Cell*& root=cells[i]; |
| Cell* pp=0; |
| Cell* pc=root; |
| while(pc) |
| { |
| Cell* pn=pc->next; |
| if(pc->pclient==pcs) |
| { |
| if(pp) pp->next=pn; else root=pn; |
| delete pc;pc=pp;++refcount; |
| } |
| pp=pc;pc=pn; |
| } |
| } |
| return(refcount); |
| } |
| // |
| btScalar Evaluate( const btVector3& x, |
| btCollisionShape* shape, |
| btVector3& normal, |
| btScalar margin) |
| { |
| /* Lookup cell */ |
| const btVector3 scx=x/voxelsz; |
| const IntFrac ix=Decompose(scx.x()); |
| const IntFrac iy=Decompose(scx.y()); |
| const IntFrac iz=Decompose(scx.z()); |
| const unsigned h=Hash(ix.b,iy.b,iz.b,shape); |
| Cell*& root=cells[static_cast<int>(h%cells.size())]; |
| Cell* c=root; |
| ++nqueries; |
| while(c) |
| { |
| ++nprobes; |
| if( (c->hash==h) && |
| (c->c[0]==ix.b) && |
| (c->c[1]==iy.b) && |
| (c->c[2]==iz.b) && |
| (c->pclient==shape)) |
| { break; } |
| else |
| { c=c->next; } |
| } |
| if(!c) |
| { |
| ++nprobes; |
| ++ncells; |
| c=new Cell(); |
| c->next=root;root=c; |
| c->pclient=shape; |
| c->hash=h; |
| c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; |
| BuildCell(*c); |
| } |
| c->puid=puid; |
| /* Extract infos */ |
| const int o[]={ ix.i,iy.i,iz.i}; |
| const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], |
| c->d[o[0]+1][o[1]+0][o[2]+0], |
| c->d[o[0]+1][o[1]+1][o[2]+0], |
| c->d[o[0]+0][o[1]+1][o[2]+0], |
| c->d[o[0]+0][o[1]+0][o[2]+1], |
| c->d[o[0]+1][o[1]+0][o[2]+1], |
| c->d[o[0]+1][o[1]+1][o[2]+1], |
| c->d[o[0]+0][o[1]+1][o[2]+1]}; |
| /* Normal */ |
| #if 1 |
| const btScalar gx[]={ d[1]-d[0],d[2]-d[3], |
| d[5]-d[4],d[6]-d[7]}; |
| const btScalar gy[]={ d[3]-d[0],d[2]-d[1], |
| d[7]-d[4],d[6]-d[5]}; |
| const btScalar gz[]={ d[4]-d[0],d[5]-d[1], |
| d[7]-d[3],d[6]-d[2]}; |
| normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), |
| Lerp(gx[2],gx[3],iy.f),iz.f)); |
| normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), |
| Lerp(gy[2],gy[3],ix.f),iz.f)); |
| normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), |
| Lerp(gz[2],gz[3],ix.f),iy.f)); |
| normal = normal.normalized(); |
| #else |
| normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); |
| #endif |
| /* Distance */ |
| const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), |
| Lerp(d[3],d[2],ix.f),iy.f); |
| const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), |
| Lerp(d[7],d[6],ix.f),iy.f); |
| return(Lerp(d0,d1,iz.f)-margin); |
| } |
| // |
| void BuildCell(Cell& c) |
| { |
| const btVector3 org=btVector3( (btScalar)c.c[0], |
| (btScalar)c.c[1], |
| (btScalar)c.c[2]) * |
| CELLSIZE*voxelsz; |
| for(int k=0;k<=CELLSIZE;++k) |
| { |
| const btScalar z=voxelsz*k+org.z(); |
| for(int j=0;j<=CELLSIZE;++j) |
| { |
| const btScalar y=voxelsz*j+org.y(); |
| for(int i=0;i<=CELLSIZE;++i) |
| { |
| const btScalar x=voxelsz*i+org.x(); |
| c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), |
| c.pclient); |
| } |
| } |
| } |
| } |
| // |
| static inline btScalar DistanceToShape(const btVector3& x, |
| btCollisionShape* shape) |
| { |
| btTransform unit; |
| unit.setIdentity(); |
| if(shape->isConvex()) |
| { |
| btGjkEpaSolver2::sResults res; |
| btConvexShape* csh=static_cast<btConvexShape*>(shape); |
| return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); |
| } |
| return(0); |
| } |
| // |
| static inline IntFrac Decompose(btScalar x) |
| { |
| /* That one need a lot of improvements... */ |
| /* Remove test, faster floor... */ |
| IntFrac r; |
| x/=CELLSIZE; |
| const int o=x<0?(int)(-x+1):0; |
| x+=o;r.b=(int)x; |
| const btScalar k=(x-r.b)*CELLSIZE; |
| r.i=(int)k;r.f=k-r.i;r.b-=o; |
| return(r); |
| } |
| // |
| static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) |
| { |
| return(a+(b-a)*t); |
| } |
| |
| |
| |
| // |
| static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape) |
| { |
| struct btS |
| { |
| int x,y,z; |
| void* p; |
| }; |
| |
| btS myset; |
| |
| myset.x=x;myset.y=y;myset.z=z;myset.p=shape; |
| const void* ptr = &myset; |
| |
| unsigned int result = HsiehHash<sizeof(btS)/4> (ptr); |
| |
| |
| return result; |
| } |
| }; |
| |
| |
| #endif |