dali-physics/third-party/bullet3/src/BulletSoftBody/btSparseSDF.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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 // Fast Hash
  24
  25 #if !defined(get16bits)
  26 #define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
  27 #endif
  28 //
  29 // super hash function by Paul Hsieh
  30 //
  31 inline unsigned int HsiehHash(const char* data, int len)
  32 {
  33         unsigned int hash = len, tmp;
  34         len >>= 2;
  35
  36         /* Main loop */
  37         for (; len > 0; len--)
  38         {
  39                 hash += get16bits(data);
  40                 tmp = (get16bits(data + 2) << 11) ^ hash;
  41                 hash = (hash << 16) ^ tmp;
  42                 data += 2 * sizeof(unsigned short);
  43                 hash += hash >> 11;
  44         }
  45
  46         /* Force "avalanching" of final 127 bits */
  47         hash ^= hash << 3;
  48         hash += hash >> 5;
  49         hash ^= hash << 4;
  50         hash += hash >> 17;
  51         hash ^= hash << 25;
  52         hash += hash >> 6;
  53
  54         return hash;
  55 }
  56
  57 template <const int CELLSIZE>
  58 struct btSparseSdf
  59 {
  60         //
  61         // Inner types
  62         //
  63         struct IntFrac
  64         {
  65                 int b;
  66                 int i;
  67                 btScalar f;
  68         };
  69         struct Cell
  70         {
  71                 btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1];
  72                 int c[3];
  73                 int puid;
  74                 unsigned hash;
  75                 const btCollisionShape* pclient;
  76                 Cell* next;
  77         };
  78         //
  79         // Fields
  80         //
  81
  82         btAlignedObjectArray<Cell*> cells;
  83         btScalar voxelsz;
  84         btScalar m_defaultVoxelsz;
  85         int puid;
  86         int ncells;
  87         int m_clampCells;
  88         int nprobes;
  89         int nqueries;
  90
  91         ~btSparseSdf()
  92         {
  93                 Reset();
  94         }
  95         //
  96         // Methods
  97         //
  98
  99         //
 100         void Initialize(int hashsize = 2383, int clampCells = 256 * 1024)
 101         {
 102                 //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated
 103                 //if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
 104                 m_clampCells = clampCells;
 105                 cells.resize(hashsize, 0);
 106                 m_defaultVoxelsz = 0.25;
 107                 Reset();
 108         }
 109         //
 110
 111         void setDefaultVoxelsz(btScalar sz)
 112         {
 113                 m_defaultVoxelsz = sz;
 114         }
 115
 116         void Reset()
 117         {
 118                 for (int i = 0, ni = cells.size(); i < ni; ++i)
 119                 {
 120                         Cell* pc = cells[i];
 121                         cells[i] = 0;
 122                         while (pc)
 123                         {
 124                                 Cell* pn = pc->next;
 125                                 delete pc;
 126                                 pc = pn;
 127                         }
 128                 }
 129                 voxelsz = m_defaultVoxelsz;
 130                 puid = 0;
 131                 ncells = 0;
 132                 nprobes = 1;
 133                 nqueries = 1;
 134         }
 135         //
 136         void GarbageCollect(int lifetime = 256)
 137         {
 138                 const int life = puid - lifetime;
 139                 for (int i = 0; i < cells.size(); ++i)
 140                 {
 141                         Cell*& root = cells[i];
 142                         Cell* pp = 0;
 143                         Cell* pc = root;
 144                         while (pc)
 145                         {
 146                                 Cell* pn = pc->next;
 147                                 if (pc->puid < life)
 148                                 {
 149                                         if (pp)
 150                                                 pp->next = pn;
 151                                         else
 152                                                 root = pn;
 153                                         delete pc;
 154                                         pc = pp;
 155                                         --ncells;
 156                                 }
 157                                 pp = pc;
 158                                 pc = pn;
 159                         }
 160                 }
 161                 //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
 162                 nqueries = 1;
 163                 nprobes = 1;
 164                 ++puid;  ///@todo: Reset puid's when int range limit is reached */
 165                 /* else setup a priority list...                                                */
 166         }
 167         //
 168         int RemoveReferences(btCollisionShape* pcs)
 169         {
 170                 int refcount = 0;
 171                 for (int i = 0; i < cells.size(); ++i)
 172                 {
 173                         Cell*& root = cells[i];
 174                         Cell* pp = 0;
 175                         Cell* pc = root;
 176                         while (pc)
 177                         {
 178                                 Cell* pn = pc->next;
 179                                 if (pc->pclient == pcs)
 180                                 {
 181                                         if (pp)
 182                                                 pp->next = pn;
 183                                         else
 184                                                 root = pn;
 185                                         delete pc;
 186                                         pc = pp;
 187                                         ++refcount;
 188                                 }
 189                                 pp = pc;
 190                                 pc = pn;
 191                         }
 192                 }
 193                 return (refcount);
 194         }
 195         //
 196         btScalar Evaluate(const btVector3& x,
 197                                           const btCollisionShape* shape,
 198                                           btVector3& normal,
 199                                           btScalar margin)
 200         {
 201                 /* Lookup cell                  */
 202                 const btVector3 scx = x / voxelsz;
 203                 const IntFrac ix = Decompose(scx.x());
 204                 const IntFrac iy = Decompose(scx.y());
 205                 const IntFrac iz = Decompose(scx.z());
 206                 const unsigned h = Hash(ix.b, iy.b, iz.b, shape);
 207                 Cell*& root = cells[static_cast<int>(h % cells.size())];
 208                 Cell* c = root;
 209                 ++nqueries;
 210                 while (c)
 211                 {
 212                         ++nprobes;
 213                         if ((c->hash == h) &&
 214                                 (c->c[0] == ix.b) &&
 215                                 (c->c[1] == iy.b) &&
 216                                 (c->c[2] == iz.b) &&
 217                                 (c->pclient == shape))
 218                         {
 219                                 break;
 220                         }
 221                         else
 222                         {
 223                                 // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]);
 224                                 //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
 225
 226                                 c = c->next;
 227                         }
 228                 }
 229                 if (!c)
 230                 {
 231                         ++nprobes;
 232                         ++ncells;
 233                         //int sz = sizeof(Cell);
 234                         if (ncells > m_clampCells)
 235                         {
 236                                 //static int numResets = 0;
 237                                 //numResets++;
 238                                 //printf("numResets=%d\n",numResets);
 239                                 Reset();
 240                         }
 241
 242                         c = new Cell();
 243                         c->next = root;
 244                         root = c;
 245                         c->pclient = shape;
 246                         c->hash = h;
 247                         c->c[0] = ix.b;
 248                         c->c[1] = iy.b;
 249                         c->c[2] = iz.b;
 250                         BuildCell(*c);
 251                 }
 252                 c->puid = puid;
 253                 /* Extract infos                */
 254                 const int o[] = {ix.i, iy.i, iz.i};
 255                 const btScalar d[] = {c->d[o[0] + 0][o[1] + 0][o[2] + 0],
 256                                                           c->d[o[0] + 1][o[1] + 0][o[2] + 0],
 257                                                           c->d[o[0] + 1][o[1] + 1][o[2] + 0],
 258                                                           c->d[o[0] + 0][o[1] + 1][o[2] + 0],
 259                                                           c->d[o[0] + 0][o[1] + 0][o[2] + 1],
 260                                                           c->d[o[0] + 1][o[1] + 0][o[2] + 1],
 261                                                           c->d[o[0] + 1][o[1] + 1][o[2] + 1],
 262                                                           c->d[o[0] + 0][o[1] + 1][o[2] + 1]};
 263                 /* Normal       */
 264 #if 1
 265                 const btScalar gx[] = {d[1] - d[0], d[2] - d[3],
 266                                                            d[5] - d[4], d[6] - d[7]};
 267                 const btScalar gy[] = {d[3] - d[0], d[2] - d[1],
 268                                                            d[7] - d[4], d[6] - d[5]};
 269                 const btScalar gz[] = {d[4] - d[0], d[5] - d[1],
 270                                                            d[7] - d[3], d[6] - d[2]};
 271                 normal.setX(Lerp(Lerp(gx[0], gx[1], iy.f),
 272                                                  Lerp(gx[2], gx[3], iy.f), iz.f));
 273                 normal.setY(Lerp(Lerp(gy[0], gy[1], ix.f),
 274                                                  Lerp(gy[2], gy[3], ix.f), iz.f));
 275                 normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f),
 276                                                  Lerp(gz[2], gz[3], ix.f), iy.f));
 277                 normal.safeNormalize();
 278 #else
 279                 normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized();
 280 #endif
 281                 /* Distance     */
 282                 const btScalar d0 = Lerp(Lerp(d[0], d[1], ix.f),
 283                                                                  Lerp(d[3], d[2], ix.f), iy.f);
 284                 const btScalar d1 = Lerp(Lerp(d[4], d[5], ix.f),
 285                                                                  Lerp(d[7], d[6], ix.f), iy.f);
 286                 return (Lerp(d0, d1, iz.f) - margin);
 287         }
 288         //
 289         void BuildCell(Cell& c)
 290         {
 291                 const btVector3 org = btVector3((btScalar)c.c[0],
 292                                                                                 (btScalar)c.c[1],
 293                                                                                 (btScalar)c.c[2]) *
 294                                                           CELLSIZE * voxelsz;
 295                 for (int k = 0; k <= CELLSIZE; ++k)
 296                 {
 297                         const btScalar z = voxelsz * k + org.z();
 298                         for (int j = 0; j <= CELLSIZE; ++j)
 299                         {
 300                                 const btScalar y = voxelsz * j + org.y();
 301                                 for (int i = 0; i <= CELLSIZE; ++i)
 302                                 {
 303                                         const btScalar x = voxelsz * i + org.x();
 304                                         c.d[i][j][k] = DistanceToShape(btVector3(x, y, z),
 305                                                                                                    c.pclient);
 306                                 }
 307                         }
 308                 }
 309         }
 310         //
 311         static inline btScalar DistanceToShape(const btVector3& x,
 312                                                                                    const btCollisionShape* shape)
 313         {
 314                 btTransform unit;
 315                 unit.setIdentity();
 316                 if (shape->isConvex())
 317                 {
 318                         btGjkEpaSolver2::sResults res;
 319                         const btConvexShape* csh = static_cast<const btConvexShape*>(shape);
 320                         return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res));
 321                 }
 322                 return (0);
 323         }
 324         //
 325         static inline IntFrac Decompose(btScalar x)
 326         {
 327                 /* That one need a lot of improvements...       */
 328                 /* Remove test, faster floor...                         */
 329                 IntFrac r;
 330                 x /= CELLSIZE;
 331                 const int o = x < 0 ? (int)(-x + 1) : 0;
 332                 x += o;
 333                 r.b = (int)x;
 334                 const btScalar k = (x - r.b) * CELLSIZE;
 335                 r.i = (int)k;
 336                 r.f = k - r.i;
 337                 r.b -= o;
 338                 return (r);
 339         }
 340         //
 341         static inline btScalar Lerp(btScalar a, btScalar b, btScalar t)
 342         {
 343                 return (a + (b - a) * t);
 344         }
 345
 346         //
 347         static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape)
 348         {
 349                 struct btS
 350                 {
 351                         int x, y, z, w;
 352                         void* p;
 353                 };
 354
 355                 btS myset;
 356                 //memset may be needed in case of additional (uninitialized) padding!
 357                 //memset(&myset, 0, sizeof(btS));
 358
 359                 myset.x = x;
 360                 myset.y = y;
 361                 myset.z = z;
 362                 myset.w = 0;
 363                 myset.p = (void*)shape;
 364                 const char* ptr = (const char*)&myset;
 365
 366                 unsigned int result = HsiehHash(ptr, sizeof(btS));
 367
 368                 return result;
 369         }
 370 };
 371
 372 #endif  //BT_SPARSE_SDF_H