src/BulletSoftBody/btSparseSDF.h

   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 */
  15 ///btSparseSdf implementation by Nathanael Presson
  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; ///@todo: Reset puid's when int range limit is reached  */
 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