Blender V2.61 - r43446

btSparseSDF.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose,
00008 including commercial applications, and to alter it and redistribute it freely,
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00016 
00017 #ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
00018 #define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
00019 
00020 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
00022 
00023 // Modified Paul Hsieh hash
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     // Inner types
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     // Fields
00066     //
00067 
00068     btAlignedObjectArray<Cell*>     cells;  
00069     btScalar                        voxelsz;
00070     int                             puid;
00071     int                             ncells;
00072     int                             nprobes;
00073     int                             nqueries;   
00074 
00075     //
00076     // Methods
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         //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
00126         nqueries=1;
00127         nprobes=1;
00128         ++puid; 
00129         /* else setup a priority list...                        */ 
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         /* Lookup cell          */ 
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         /* Extract infos        */ 
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         /* Normal   */ 
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         /* Distance */ 
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         /* That one need a lot of improvements...   */
00267         /* Remove test, faster floor...             */ 
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