dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btMiniSDF.cpp

   1 #include "btMiniSDF.h"
   2
   3 //
   4 //Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid
   5 //example:
   6 //GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj
   7 //The MIT License (MIT)
   8 //
   9 //Copyright (c) 2017 Dan Koschier
  10 //
  11
  12 #include <limits.h>
  13 #include <string.h>  //memcpy
  14
  15 struct btSdfDataStream
  16 {
  17         const char* m_data;
  18         int m_size;
  19
  20         int m_currentOffset;
  21
  22         btSdfDataStream(const char* data, int size)
  23                 : m_data(data),
  24                   m_size(size),
  25                   m_currentOffset(0)
  26         {
  27         }
  28
  29         template <class T>
  30         bool read(T& val)
  31         {
  32                 int bytes = sizeof(T);
  33                 if (m_currentOffset + bytes <= m_size)
  34                 {
  35                         char* dest = (char*)&val;
  36                         memcpy(dest, &m_data[m_currentOffset], bytes);
  37                         m_currentOffset += bytes;
  38                         return true;
  39                 }
  40                 btAssert(0);
  41                 return false;
  42         }
  43 };
  44
  45 bool btMiniSDF::load(const char* data, int size)
  46 {
  47         int fileSize = -1;
  48
  49         btSdfDataStream ds(data, size);
  50         {
  51                 double buf[6];
  52                 ds.read(buf);
  53                 m_domain.m_min[0] = buf[0];
  54                 m_domain.m_min[1] = buf[1];
  55                 m_domain.m_min[2] = buf[2];
  56                 m_domain.m_min[3] = 0;
  57                 m_domain.m_max[0] = buf[3];
  58                 m_domain.m_max[1] = buf[4];
  59                 m_domain.m_max[2] = buf[5];
  60                 m_domain.m_max[3] = 0;
  61         }
  62         {
  63                 unsigned int buf2[3];
  64                 ds.read(buf2);
  65                 m_resolution[0] = buf2[0];
  66                 m_resolution[1] = buf2[1];
  67                 m_resolution[2] = buf2[2];
  68         }
  69         {
  70                 double buf[3];
  71                 ds.read(buf);
  72                 m_cell_size[0] = buf[0];
  73                 m_cell_size[1] = buf[1];
  74                 m_cell_size[2] = buf[2];
  75         }
  76         {
  77                 double buf[3];
  78                 ds.read(buf);
  79                 m_inv_cell_size[0] = buf[0];
  80                 m_inv_cell_size[1] = buf[1];
  81                 m_inv_cell_size[2] = buf[2];
  82         }
  83         {
  84                 unsigned long long int cells;
  85                 ds.read(cells);
  86                 m_n_cells = cells;
  87         }
  88         {
  89                 unsigned long long int fields;
  90                 ds.read(fields);
  91                 m_n_fields = fields;
  92         }
  93
  94         unsigned long long int nodes0;
  95         std::size_t n_nodes0;
  96         ds.read(nodes0);
  97         n_nodes0 = nodes0;
  98         if (n_nodes0 > 1024 * 1024 * 1024)
  99         {
 100                 return m_isValid;
 101         }
 102         m_nodes.resize(n_nodes0);
 103         for (unsigned int i = 0; i < n_nodes0; i++)
 104         {
 105                 unsigned long long int n_nodes1;
 106                 ds.read(n_nodes1);
 107                 btAlignedObjectArray<double>& nodes = m_nodes[i];
 108                 nodes.resize(n_nodes1);
 109                 for (int j = 0; j < nodes.size(); j++)
 110                 {
 111                         double& node = nodes[j];
 112                         ds.read(node);
 113                 }
 114         }
 115
 116         unsigned long long int n_cells0;
 117         ds.read(n_cells0);
 118         m_cells.resize(n_cells0);
 119         for (int i = 0; i < n_cells0; i++)
 120         {
 121                 unsigned long long int n_cells1;
 122                 btAlignedObjectArray<btCell32>& cells = m_cells[i];
 123                 ds.read(n_cells1);
 124                 cells.resize(n_cells1);
 125                 for (int j = 0; j < n_cells1; j++)
 126                 {
 127                         btCell32& cell = cells[j];
 128                         ds.read(cell);
 129                 }
 130         }
 131
 132         {
 133                 unsigned long long int n_cell_maps0;
 134                 ds.read(n_cell_maps0);
 135
 136                 m_cell_map.resize(n_cell_maps0);
 137                 for (int i = 0; i < n_cell_maps0; i++)
 138                 {
 139                         unsigned long long int n_cell_maps1;
 140                         btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i];
 141                         ds.read(n_cell_maps1);
 142                         cell_maps.resize(n_cell_maps1);
 143                         for (int j = 0; j < n_cell_maps1; j++)
 144                         {
 145                                 unsigned int& cell_map = cell_maps[j];
 146                                 ds.read(cell_map);
 147                         }
 148                 }
 149         }
 150
 151         m_isValid = (ds.m_currentOffset == ds.m_size);
 152         return m_isValid;
 153 }
 154
 155 unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const& ijk) const
 156 {
 157         return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0];
 158 }
 159
 160 btAlignedBox3d
 161 btMiniSDF::subdomain(btMultiIndex const& ijk) const
 162 {
 163         btAssert(m_isValid);
 164         btVector3 tmp;
 165         tmp.m_floats[0] = m_cell_size[0] * (double)ijk.ijk[0];
 166         tmp.m_floats[1] = m_cell_size[1] * (double)ijk.ijk[1];
 167         tmp.m_floats[2] = m_cell_size[2] * (double)ijk.ijk[2];
 168
 169         btVector3 origin = m_domain.min() + tmp;
 170
 171         btAlignedBox3d box = btAlignedBox3d(origin, origin + m_cell_size);
 172         return box;
 173 }
 174
 175 btMultiIndex
 176 btMiniSDF::singleToMultiIndex(unsigned int l) const
 177 {
 178         btAssert(m_isValid);
 179         unsigned int n01 = m_resolution[0] * m_resolution[1];
 180         unsigned int k = l / n01;
 181         unsigned int temp = l % n01;
 182         unsigned int j = temp / m_resolution[0];
 183         unsigned int i = temp % m_resolution[0];
 184         btMultiIndex mi;
 185         mi.ijk[0] = i;
 186         mi.ijk[1] = j;
 187         mi.ijk[2] = k;
 188         return mi;
 189 }
 190
 191 btAlignedBox3d
 192 btMiniSDF::subdomain(unsigned int l) const
 193 {
 194         btAssert(m_isValid);
 195         return subdomain(singleToMultiIndex(l));
 196 }
 197
 198 btShapeMatrix
 199 btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const
 200 {
 201         btAssert(m_isValid);
 202         btShapeMatrix res;
 203
 204         btScalar x = xi[0];
 205         btScalar y = xi[1];
 206         btScalar z = xi[2];
 207
 208         btScalar x2 = x * x;
 209         btScalar y2 = y * y;
 210         btScalar z2 = z * z;
 211
 212         btScalar _1mx = 1.0 - x;
 213         btScalar _1my = 1.0 - y;
 214         btScalar _1mz = 1.0 - z;
 215
 216         btScalar _1px = 1.0 + x;
 217         btScalar _1py = 1.0 + y;
 218         btScalar _1pz = 1.0 + z;
 219
 220         btScalar _1m3x = 1.0 - 3.0 * x;
 221         btScalar _1m3y = 1.0 - 3.0 * y;
 222         btScalar _1m3z = 1.0 - 3.0 * z;
 223
 224         btScalar _1p3x = 1.0 + 3.0 * x;
 225         btScalar _1p3y = 1.0 + 3.0 * y;
 226         btScalar _1p3z = 1.0 + 3.0 * z;
 227
 228         btScalar _1mxt1my = _1mx * _1my;
 229         btScalar _1mxt1py = _1mx * _1py;
 230         btScalar _1pxt1my = _1px * _1my;
 231         btScalar _1pxt1py = _1px * _1py;
 232
 233         btScalar _1mxt1mz = _1mx * _1mz;
 234         btScalar _1mxt1pz = _1mx * _1pz;
 235         btScalar _1pxt1mz = _1px * _1mz;
 236         btScalar _1pxt1pz = _1px * _1pz;
 237
 238         btScalar _1myt1mz = _1my * _1mz;
 239         btScalar _1myt1pz = _1my * _1pz;
 240         btScalar _1pyt1mz = _1py * _1mz;
 241         btScalar _1pyt1pz = _1py * _1pz;
 242
 243         btScalar _1mx2 = 1.0 - x2;
 244         btScalar _1my2 = 1.0 - y2;
 245         btScalar _1mz2 = 1.0 - z2;
 246
 247         // Corner nodes.
 248         btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0);
 249         res[0] = fac * _1mxt1my * _1mz;
 250         res[1] = fac * _1pxt1my * _1mz;
 251         res[2] = fac * _1mxt1py * _1mz;
 252         res[3] = fac * _1pxt1py * _1mz;
 253         res[4] = fac * _1mxt1my * _1pz;
 254         res[5] = fac * _1pxt1my * _1pz;
 255         res[6] = fac * _1mxt1py * _1pz;
 256         res[7] = fac * _1pxt1py * _1pz;
 257
 258         // Edge nodes.
 259
 260         fac = 9.0 / 64.0 * _1mx2;
 261         btScalar fact1m3x = fac * _1m3x;
 262         btScalar fact1p3x = fac * _1p3x;
 263         res[8] = fact1m3x * _1myt1mz;
 264         res[9] = fact1p3x * _1myt1mz;
 265         res[10] = fact1m3x * _1myt1pz;
 266         res[11] = fact1p3x * _1myt1pz;
 267         res[12] = fact1m3x * _1pyt1mz;
 268         res[13] = fact1p3x * _1pyt1mz;
 269         res[14] = fact1m3x * _1pyt1pz;
 270         res[15] = fact1p3x * _1pyt1pz;
 271
 272         fac = 9.0 / 64.0 * _1my2;
 273         btScalar fact1m3y = fac * _1m3y;
 274         btScalar fact1p3y = fac * _1p3y;
 275         res[16] = fact1m3y * _1mxt1mz;
 276         res[17] = fact1p3y * _1mxt1mz;
 277         res[18] = fact1m3y * _1pxt1mz;
 278         res[19] = fact1p3y * _1pxt1mz;
 279         res[20] = fact1m3y * _1mxt1pz;
 280         res[21] = fact1p3y * _1mxt1pz;
 281         res[22] = fact1m3y * _1pxt1pz;
 282         res[23] = fact1p3y * _1pxt1pz;
 283
 284         fac = 9.0 / 64.0 * _1mz2;
 285         btScalar fact1m3z = fac * _1m3z;
 286         btScalar fact1p3z = fac * _1p3z;
 287         res[24] = fact1m3z * _1mxt1my;
 288         res[25] = fact1p3z * _1mxt1my;
 289         res[26] = fact1m3z * _1mxt1py;
 290         res[27] = fact1p3z * _1mxt1py;
 291         res[28] = fact1m3z * _1pxt1my;
 292         res[29] = fact1p3z * _1pxt1my;
 293         res[30] = fact1m3z * _1pxt1py;
 294         res[31] = fact1p3z * _1pxt1py;
 295
 296         if (gradient)
 297         {
 298                 btShapeGradients& dN = *gradient;
 299
 300                 btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0;
 301                 btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0;
 302                 btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0;
 303                 btScalar _18x = 18.0 * x;
 304                 btScalar _18y = 18.0 * y;
 305                 btScalar _18z = 18.0 * z;
 306
 307                 btScalar _3m9x2 = 3.0 - 9.0 * x2;
 308                 btScalar _3m9y2 = 3.0 - 9.0 * y2;
 309                 btScalar _3m9z2 = 3.0 - 9.0 * z2;
 310
 311                 btScalar _2x = 2.0 * x;
 312                 btScalar _2y = 2.0 * y;
 313                 btScalar _2z = 2.0 * z;
 314
 315                 btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19;
 316                 btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19;
 317                 btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19;
 318                 btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19;
 319                 btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19;
 320                 btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19;
 321
 322                 dN(0, 0) = _18xm9t3x2py2pz2m19 * _1myt1mz;
 323                 dN(0, 1) = _1mxt1mz * _18ym9tx2p3y2pz2m19;
 324                 dN(0, 2) = _1mxt1my * _18zm9tx2py2p3z2m19;
 325                 dN(1, 0) = _18xp9t3x2py2pz2m19 * _1myt1mz;
 326                 dN(1, 1) = _1pxt1mz * _18ym9tx2p3y2pz2m19;
 327                 dN(1, 2) = _1pxt1my * _18zm9tx2py2p3z2m19;
 328                 dN(2, 0) = _18xm9t3x2py2pz2m19 * _1pyt1mz;
 329                 dN(2, 1) = _1mxt1mz * _18yp9tx2p3y2pz2m19;
 330                 dN(2, 2) = _1mxt1py * _18zm9tx2py2p3z2m19;
 331                 dN(3, 0) = _18xp9t3x2py2pz2m19 * _1pyt1mz;
 332                 dN(3, 1) = _1pxt1mz * _18yp9tx2p3y2pz2m19;
 333                 dN(3, 2) = _1pxt1py * _18zm9tx2py2p3z2m19;
 334                 dN(4, 0) = _18xm9t3x2py2pz2m19 * _1myt1pz;
 335                 dN(4, 1) = _1mxt1pz * _18ym9tx2p3y2pz2m19;
 336                 dN(4, 2) = _1mxt1my * _18zp9tx2py2p3z2m19;
 337                 dN(5, 0) = _18xp9t3x2py2pz2m19 * _1myt1pz;
 338                 dN(5, 1) = _1pxt1pz * _18ym9tx2p3y2pz2m19;
 339                 dN(5, 2) = _1pxt1my * _18zp9tx2py2p3z2m19;
 340                 dN(6, 0) = _18xm9t3x2py2pz2m19 * _1pyt1pz;
 341                 dN(6, 1) = _1mxt1pz * _18yp9tx2p3y2pz2m19;
 342                 dN(6, 2) = _1mxt1py * _18zp9tx2py2p3z2m19;
 343                 dN(7, 0) = _18xp9t3x2py2pz2m19 * _1pyt1pz;
 344                 dN(7, 1) = _1pxt1pz * _18yp9tx2p3y2pz2m19;
 345                 dN(7, 2) = _1pxt1py * _18zp9tx2py2p3z2m19;
 346
 347                 dN.topRowsDivide(8, 64.0);
 348
 349                 btScalar _m3m9x2m2x = -_3m9x2 - _2x;
 350                 btScalar _p3m9x2m2x = _3m9x2 - _2x;
 351                 btScalar _1mx2t1m3x = _1mx2 * _1m3x;
 352                 btScalar _1mx2t1p3x = _1mx2 * _1p3x;
 353                 dN(8, 0) = _m3m9x2m2x * _1myt1mz,
 354                           dN(8, 1) = -_1mx2t1m3x * _1mz,
 355                           dN(8, 2) = -_1mx2t1m3x * _1my;
 356                 dN(9, 0) = _p3m9x2m2x * _1myt1mz,
 357                           dN(9, 1) = -_1mx2t1p3x * _1mz,
 358                           dN(9, 2) = -_1mx2t1p3x * _1my;
 359                 dN(10, 0) = _m3m9x2m2x * _1myt1pz,
 360                            dN(10, 1) = -_1mx2t1m3x * _1pz,
 361                            dN(10, 2) = _1mx2t1m3x * _1my;
 362                 dN(11, 0) = _p3m9x2m2x * _1myt1pz,
 363                            dN(11, 1) = -_1mx2t1p3x * _1pz,
 364                            dN(11, 2) = _1mx2t1p3x * _1my;
 365                 dN(12, 0) = _m3m9x2m2x * _1pyt1mz,
 366                            dN(12, 1) = _1mx2t1m3x * _1mz,
 367                            dN(12, 2) = -_1mx2t1m3x * _1py;
 368                 dN(13, 0) = _p3m9x2m2x * _1pyt1mz,
 369                            dN(13, 1) = _1mx2t1p3x * _1mz,
 370                            dN(13, 2) = -_1mx2t1p3x * _1py;
 371                 dN(14, 0) = _m3m9x2m2x * _1pyt1pz,
 372                            dN(14, 1) = _1mx2t1m3x * _1pz,
 373                            dN(14, 2) = _1mx2t1m3x * _1py;
 374                 dN(15, 0) = _p3m9x2m2x * _1pyt1pz,
 375                            dN(15, 1) = _1mx2t1p3x * _1pz,
 376                            dN(15, 2) = _1mx2t1p3x * _1py;
 377
 378                 btScalar _m3m9y2m2y = -_3m9y2 - _2y;
 379                 btScalar _p3m9y2m2y = _3m9y2 - _2y;
 380                 btScalar _1my2t1m3y = _1my2 * _1m3y;
 381                 btScalar _1my2t1p3y = _1my2 * _1p3y;
 382                 dN(16, 0) = -_1my2t1m3y * _1mz,
 383                            dN(16, 1) = _m3m9y2m2y * _1mxt1mz,
 384                            dN(16, 2) = -_1my2t1m3y * _1mx;
 385                 dN(17, 0) = -_1my2t1p3y * _1mz,
 386                            dN(17, 1) = _p3m9y2m2y * _1mxt1mz,
 387                            dN(17, 2) = -_1my2t1p3y * _1mx;
 388                 dN(18, 0) = _1my2t1m3y * _1mz,
 389                            dN(18, 1) = _m3m9y2m2y * _1pxt1mz,
 390                            dN(18, 2) = -_1my2t1m3y * _1px;
 391                 dN(19, 0) = _1my2t1p3y * _1mz,
 392                            dN(19, 1) = _p3m9y2m2y * _1pxt1mz,
 393                            dN(19, 2) = -_1my2t1p3y * _1px;
 394                 dN(20, 0) = -_1my2t1m3y * _1pz,
 395                            dN(20, 1) = _m3m9y2m2y * _1mxt1pz,
 396                            dN(20, 2) = _1my2t1m3y * _1mx;
 397                 dN(21, 0) = -_1my2t1p3y * _1pz,
 398                            dN(21, 1) = _p3m9y2m2y * _1mxt1pz,
 399                            dN(21, 2) = _1my2t1p3y * _1mx;
 400                 dN(22, 0) = _1my2t1m3y * _1pz,
 401                            dN(22, 1) = _m3m9y2m2y * _1pxt1pz,
 402                            dN(22, 2) = _1my2t1m3y * _1px;
 403                 dN(23, 0) = _1my2t1p3y * _1pz,
 404                            dN(23, 1) = _p3m9y2m2y * _1pxt1pz,
 405                            dN(23, 2) = _1my2t1p3y * _1px;
 406
 407                 btScalar _m3m9z2m2z = -_3m9z2 - _2z;
 408                 btScalar _p3m9z2m2z = _3m9z2 - _2z;
 409                 btScalar _1mz2t1m3z = _1mz2 * _1m3z;
 410                 btScalar _1mz2t1p3z = _1mz2 * _1p3z;
 411                 dN(24, 0) = -_1mz2t1m3z * _1my,
 412                            dN(24, 1) = -_1mz2t1m3z * _1mx,
 413                            dN(24, 2) = _m3m9z2m2z * _1mxt1my;
 414                 dN(25, 0) = -_1mz2t1p3z * _1my,
 415                            dN(25, 1) = -_1mz2t1p3z * _1mx,
 416                            dN(25, 2) = _p3m9z2m2z * _1mxt1my;
 417                 dN(26, 0) = -_1mz2t1m3z * _1py,
 418                            dN(26, 1) = _1mz2t1m3z * _1mx,
 419                            dN(26, 2) = _m3m9z2m2z * _1mxt1py;
 420                 dN(27, 0) = -_1mz2t1p3z * _1py,
 421                            dN(27, 1) = _1mz2t1p3z * _1mx,
 422                            dN(27, 2) = _p3m9z2m2z * _1mxt1py;
 423                 dN(28, 0) = _1mz2t1m3z * _1my,
 424                            dN(28, 1) = -_1mz2t1m3z * _1px,
 425                            dN(28, 2) = _m3m9z2m2z * _1pxt1my;
 426                 dN(29, 0) = _1mz2t1p3z * _1my,
 427                            dN(29, 1) = -_1mz2t1p3z * _1px,
 428                            dN(29, 2) = _p3m9z2m2z * _1pxt1my;
 429                 dN(30, 0) = _1mz2t1m3z * _1py,
 430                            dN(30, 1) = _1mz2t1m3z * _1px,
 431                            dN(30, 2) = _m3m9z2m2z * _1pxt1py;
 432                 dN(31, 0) = _1mz2t1p3z * _1py,
 433                            dN(31, 1) = _1mz2t1p3z * _1px,
 434                            dN(31, 2) = _p3m9z2m2z * _1pxt1py;
 435
 436                 dN.bottomRowsMul(32u - 8u, 9.0 / 64.0);
 437         }
 438
 439         return res;
 440 }
 441
 442 bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x,
 443                                                         btVector3* gradient) const
 444 {
 445         btAssert(m_isValid);
 446         if (!m_isValid)
 447                 return false;
 448
 449         if (!m_domain.contains(x))
 450                 return false;
 451
 452         btVector3 tmpmi = ((x - m_domain.min()) * (m_inv_cell_size));  //.cast<unsigned int>().eval();
 453         unsigned int mi[3] = {(unsigned int)tmpmi[0], (unsigned int)tmpmi[1], (unsigned int)tmpmi[2]};
 454         if (mi[0] >= m_resolution[0])
 455                 mi[0] = m_resolution[0] - 1;
 456         if (mi[1] >= m_resolution[1])
 457                 mi[1] = m_resolution[1] - 1;
 458         if (mi[2] >= m_resolution[2])
 459                 mi[2] = m_resolution[2] - 1;
 460         btMultiIndex mui;
 461         mui.ijk[0] = mi[0];
 462         mui.ijk[1] = mi[1];
 463         mui.ijk[2] = mi[2];
 464         int i = multiToSingleIndex(mui);
 465         unsigned int i_ = m_cell_map[field_id][i];
 466         if (i_ == UINT_MAX)
 467                 return false;
 468
 469         btAlignedBox3d sd = subdomain(i);
 470         i = i_;
 471         btVector3 d = sd.m_max - sd.m_min;  //.diagonal().eval();
 472
 473         btVector3 denom = (sd.max() - sd.min());
 474         btVector3 c0 = btVector3(2.0, 2.0, 2.0) / denom;
 475         btVector3 c1 = (sd.max() + sd.min()) / denom;
 476         btVector3 xi = (c0 * x - c1);
 477
 478         btCell32 const& cell = m_cells[field_id][i];
 479         if (!gradient)
 480         {
 481                 //auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0));
 482                 double phi = 0.0;
 483                 btShapeMatrix N = shape_function_(xi, 0);
 484                 for (unsigned int j = 0u; j < 32u; ++j)
 485                 {
 486                         unsigned int v = cell.m_cells[j];
 487                         double c = m_nodes[field_id][v];
 488                         if (c == DBL_MAX)
 489                         {
 490                                 return false;
 491                                 ;
 492                         }
 493                         phi += c * N[j];
 494                 }
 495
 496                 dist = phi;
 497                 return true;
 498         }
 499
 500         btShapeGradients dN;
 501         btShapeMatrix N = shape_function_(xi, &dN);
 502
 503         double phi = 0.0;
 504         gradient->setZero();
 505         for (unsigned int j = 0u; j < 32u; ++j)
 506         {
 507                 unsigned int v = cell.m_cells[j];
 508                 double c = m_nodes[field_id][v];
 509                 if (c == DBL_MAX)
 510                 {
 511                         gradient->setZero();
 512                         return false;
 513                 }
 514                 phi += c * N[j];
 515                 (*gradient)[0] += c * dN(j, 0);
 516                 (*gradient)[1] += c * dN(j, 1);
 517                 (*gradient)[2] += c * dN(j, 2);
 518         }
 519         (*gradient) *= c0;
 520         dist = phi;
 521         return true;
 522 }