diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp | 532 |
1 files changed, 532 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp new file mode 100644 index 0000000000..afe45e1d2d --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp @@ -0,0 +1,532 @@ +#include "btMiniSDF.h" + +// +//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid +//example: +//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj +//The MIT License (MIT) +// +//Copyright (c) 2017 Dan Koschier +// + +#include <limits.h> +#include <string.h> //memcpy + +struct btSdfDataStream +{ + const char* m_data; + int m_size; + + int m_currentOffset; + + btSdfDataStream(const char* data, int size) + :m_data(data), + m_size(size), + m_currentOffset(0) + { + + } + + template<class T> bool read(T& val) + { + int bytes = sizeof(T); + if (m_currentOffset+bytes<=m_size) + { + char* dest = (char*)&val; + memcpy(dest,&m_data[m_currentOffset],bytes); + m_currentOffset+=bytes; + return true; + } + btAssert(0); + return false; + } +}; + + +bool btMiniSDF::load(const char* data, int size) +{ + int fileSize = -1; + + btSdfDataStream ds(data,size); + { + double buf[6]; + ds.read(buf); + m_domain.m_min[0] = buf[0]; + m_domain.m_min[1] = buf[1]; + m_domain.m_min[2] = buf[2]; + m_domain.m_min[3] = 0; + m_domain.m_max[0] = buf[3]; + m_domain.m_max[1] = buf[4]; + m_domain.m_max[2] = buf[5]; + m_domain.m_max[3] = 0; + } + { + unsigned int buf2[3]; + ds.read(buf2); + m_resolution[0] = buf2[0]; + m_resolution[1] = buf2[1]; + m_resolution[2] = buf2[2]; + } + { + double buf[3]; + ds.read(buf); + m_cell_size[0] = buf[0]; + m_cell_size[1] = buf[1]; + m_cell_size[2] = buf[2]; + } + { + double buf[3]; + ds.read(buf); + m_inv_cell_size[0] = buf[0]; + m_inv_cell_size[1] = buf[1]; + m_inv_cell_size[2] = buf[2]; + } + { + unsigned long long int cells; + ds.read(cells); + m_n_cells = cells; + } + { + unsigned long long int fields; + ds.read(fields); + m_n_fields = fields; + } + + + unsigned long long int nodes0; + std::size_t n_nodes0; + ds.read(nodes0); + n_nodes0 = nodes0; + if (n_nodes0 > 1024 * 1024 * 1024) + { + return m_isValid; + } + m_nodes.resize(n_nodes0); + for (unsigned int i=0;i<n_nodes0;i++) + { + unsigned long long int n_nodes1; + ds.read(n_nodes1); + btAlignedObjectArray<double>& nodes = m_nodes[i]; + nodes.resize(n_nodes1); + for ( int j=0;j<nodes.size();j++) + { + double& node = nodes[j]; + ds.read(node); + } + } + + unsigned long long int n_cells0; + ds.read(n_cells0); + m_cells.resize(n_cells0); + for (int i=0;i<n_cells0;i++) + { + unsigned long long int n_cells1; + btAlignedObjectArray<btCell32 >& cells = m_cells[i]; + ds.read(n_cells1); + cells.resize(n_cells1); + for (int j=0;j<n_cells1;j++) + { + btCell32& cell = cells[j]; + ds.read(cell); + } + } + + { + unsigned long long int n_cell_maps0; + ds.read(n_cell_maps0); + + m_cell_map.resize(n_cell_maps0); + for (int i=0;i<n_cell_maps0;i++) + { + unsigned long long int n_cell_maps1; + btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i]; + ds.read(n_cell_maps1); + cell_maps.resize(n_cell_maps1); + for (int j=0;j<n_cell_maps1;j++) + { + unsigned int& cell_map = cell_maps[j]; + ds.read(cell_map); + } + } + } + + m_isValid = (ds.m_currentOffset == ds.m_size); + return m_isValid; +} + + +unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const & ijk) const +{ + return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0]; +} + +btAlignedBox3d +btMiniSDF::subdomain(btMultiIndex const& ijk) const +{ + btAssert(m_isValid); + btVector3 tmp; + tmp.m_floats[0] = m_cell_size[0]*(double)ijk.ijk[0]; + tmp.m_floats[1] = m_cell_size[1]*(double)ijk.ijk[1]; + tmp.m_floats[2] = m_cell_size[2]*(double)ijk.ijk[2]; + + + btVector3 origin = m_domain.min() + tmp; + + btAlignedBox3d box = btAlignedBox3d (origin, origin + m_cell_size); + return box; +} + +btMultiIndex +btMiniSDF::singleToMultiIndex(unsigned int l) const +{ + btAssert(m_isValid); + unsigned int n01 = m_resolution[0] * m_resolution[1]; + unsigned int k = l / n01; + unsigned int temp = l % n01; + unsigned int j = temp / m_resolution[0]; + unsigned int i = temp % m_resolution[0]; + btMultiIndex mi; + mi.ijk[0] = i; + mi.ijk[1] = j; + mi.ijk[2] = k; + return mi; +} + +btAlignedBox3d +btMiniSDF::subdomain(unsigned int l) const +{ + btAssert(m_isValid); + return subdomain(singleToMultiIndex(l)); +} + + +btShapeMatrix +btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const +{ + btAssert(m_isValid); + btShapeMatrix res; + + btScalar x = xi[0]; + btScalar y = xi[1]; + btScalar z = xi[2]; + + btScalar x2 = x*x; + btScalar y2 = y*y; + btScalar z2 = z*z; + + btScalar _1mx = 1.0 - x; + btScalar _1my = 1.0 - y; + btScalar _1mz = 1.0 - z; + + btScalar _1px = 1.0 + x; + btScalar _1py = 1.0 + y; + btScalar _1pz = 1.0 + z; + + btScalar _1m3x = 1.0 - 3.0 * x; + btScalar _1m3y = 1.0 - 3.0 * y; + btScalar _1m3z = 1.0 - 3.0 * z; + + btScalar _1p3x = 1.0 + 3.0 * x; + btScalar _1p3y = 1.0 + 3.0 * y; + btScalar _1p3z = 1.0 + 3.0 * z; + + btScalar _1mxt1my = _1mx * _1my; + btScalar _1mxt1py = _1mx * _1py; + btScalar _1pxt1my = _1px * _1my; + btScalar _1pxt1py = _1px * _1py; + + btScalar _1mxt1mz = _1mx * _1mz; + btScalar _1mxt1pz = _1mx * _1pz; + btScalar _1pxt1mz = _1px * _1mz; + btScalar _1pxt1pz = _1px * _1pz; + + btScalar _1myt1mz = _1my * _1mz; + btScalar _1myt1pz = _1my * _1pz; + btScalar _1pyt1mz = _1py * _1mz; + btScalar _1pyt1pz = _1py * _1pz; + + btScalar _1mx2 = 1.0 - x2; + btScalar _1my2 = 1.0 - y2; + btScalar _1mz2 = 1.0 - z2; + + + // Corner nodes. + btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0); + res[0] = fac * _1mxt1my * _1mz; + res[1] = fac * _1pxt1my * _1mz; + res[2] = fac * _1mxt1py * _1mz; + res[3] = fac * _1pxt1py * _1mz; + res[4] = fac * _1mxt1my * _1pz; + res[5] = fac * _1pxt1my * _1pz; + res[6] = fac * _1mxt1py * _1pz; + res[7] = fac * _1pxt1py * _1pz; + + // Edge nodes. + + fac = 9.0 / 64.0 * _1mx2; + btScalar fact1m3x = fac * _1m3x; + btScalar fact1p3x = fac * _1p3x; + res[ 8] = fact1m3x * _1myt1mz; + res[ 9] = fact1p3x * _1myt1mz; + res[10] = fact1m3x * _1myt1pz; + res[11] = fact1p3x * _1myt1pz; + res[12] = fact1m3x * _1pyt1mz; + res[13] = fact1p3x * _1pyt1mz; + res[14] = fact1m3x * _1pyt1pz; + res[15] = fact1p3x * _1pyt1pz; + + fac = 9.0 / 64.0 * _1my2; + btScalar fact1m3y = fac * _1m3y; + btScalar fact1p3y = fac * _1p3y; + res[16] = fact1m3y * _1mxt1mz; + res[17] = fact1p3y * _1mxt1mz; + res[18] = fact1m3y * _1pxt1mz; + res[19] = fact1p3y * _1pxt1mz; + res[20] = fact1m3y * _1mxt1pz; + res[21] = fact1p3y * _1mxt1pz; + res[22] = fact1m3y * _1pxt1pz; + res[23] = fact1p3y * _1pxt1pz; + + fac = 9.0 / 64.0 * _1mz2; + btScalar fact1m3z = fac * _1m3z; + btScalar fact1p3z = fac * _1p3z; + res[24] = fact1m3z * _1mxt1my; + res[25] = fact1p3z * _1mxt1my; + res[26] = fact1m3z * _1mxt1py; + res[27] = fact1p3z * _1mxt1py; + res[28] = fact1m3z * _1pxt1my; + res[29] = fact1p3z * _1pxt1my; + res[30] = fact1m3z * _1pxt1py; + res[31] = fact1p3z * _1pxt1py; + + if (gradient) + { + btShapeGradients& dN = *gradient; + + btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0; + btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0; + btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0; + btScalar _18x = 18.0 * x; + btScalar _18y = 18.0 * y; + btScalar _18z = 18.0 * z; + + btScalar _3m9x2 = 3.0 - 9.0 * x2; + btScalar _3m9y2 = 3.0 - 9.0 * y2; + btScalar _3m9z2 = 3.0 - 9.0 * z2; + + btScalar _2x = 2.0 * x; + btScalar _2y = 2.0 * y; + btScalar _2z = 2.0 * z; + + btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19; + btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19; + btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19; + btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19; + btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19; + btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19; + + dN(0,0) =_18xm9t3x2py2pz2m19 * _1myt1mz; + dN(0,1) =_1mxt1mz * _18ym9tx2p3y2pz2m19; + dN(0,2) =_1mxt1my * _18zm9tx2py2p3z2m19; + dN(1,0) =_18xp9t3x2py2pz2m19 * _1myt1mz; + dN(1,1) =_1pxt1mz * _18ym9tx2p3y2pz2m19; + dN(1,2) =_1pxt1my * _18zm9tx2py2p3z2m19; + dN(2,0) =_18xm9t3x2py2pz2m19 * _1pyt1mz; + dN(2,1) =_1mxt1mz * _18yp9tx2p3y2pz2m19; + dN(2,2) =_1mxt1py * _18zm9tx2py2p3z2m19; + dN(3,0) =_18xp9t3x2py2pz2m19 * _1pyt1mz; + dN(3,1) =_1pxt1mz * _18yp9tx2p3y2pz2m19; + dN(3,2) =_1pxt1py * _18zm9tx2py2p3z2m19; + dN(4,0) =_18xm9t3x2py2pz2m19 * _1myt1pz; + dN(4,1) =_1mxt1pz * _18ym9tx2p3y2pz2m19; + dN(4,2) =_1mxt1my * _18zp9tx2py2p3z2m19; + dN(5,0) =_18xp9t3x2py2pz2m19 * _1myt1pz; + dN(5,1) =_1pxt1pz * _18ym9tx2p3y2pz2m19; + dN(5,2) =_1pxt1my * _18zp9tx2py2p3z2m19; + dN(6,0) =_18xm9t3x2py2pz2m19 * _1pyt1pz; + dN(6,1) =_1mxt1pz * _18yp9tx2p3y2pz2m19; + dN(6,2) =_1mxt1py * _18zp9tx2py2p3z2m19; + dN(7,0) =_18xp9t3x2py2pz2m19 * _1pyt1pz; + dN(7,1) =_1pxt1pz * _18yp9tx2p3y2pz2m19; + dN(7,2) =_1pxt1py * _18zp9tx2py2p3z2m19; + + dN.topRowsDivide(8, 64.0); + + btScalar _m3m9x2m2x = -_3m9x2 - _2x; + btScalar _p3m9x2m2x = _3m9x2 - _2x; + btScalar _1mx2t1m3x = _1mx2 * _1m3x; + btScalar _1mx2t1p3x = _1mx2 * _1p3x; + dN( 8,0) = _m3m9x2m2x * _1myt1mz, + dN( 8,1) = -_1mx2t1m3x * _1mz, + dN( 8,2) = -_1mx2t1m3x * _1my; + dN( 9,0) = _p3m9x2m2x * _1myt1mz, + dN( 9,1) = -_1mx2t1p3x * _1mz, + dN( 9,2) = -_1mx2t1p3x * _1my; + dN(10,0) = _m3m9x2m2x * _1myt1pz, + dN(10,1) = -_1mx2t1m3x * _1pz, + dN(10,2) = _1mx2t1m3x * _1my; + dN(11,0) = _p3m9x2m2x * _1myt1pz, + dN(11,1) = -_1mx2t1p3x * _1pz, + dN(11,2) = _1mx2t1p3x * _1my; + dN(12,0) = _m3m9x2m2x * _1pyt1mz, + dN(12,1) = _1mx2t1m3x * _1mz, + dN(12,2) = -_1mx2t1m3x * _1py; + dN(13,0) = _p3m9x2m2x * _1pyt1mz, + dN(13,1) = _1mx2t1p3x * _1mz, + dN(13,2) = -_1mx2t1p3x * _1py; + dN(14,0) = _m3m9x2m2x * _1pyt1pz, + dN(14,1) = _1mx2t1m3x * _1pz, + dN(14,2) = _1mx2t1m3x * _1py; + dN(15,0) = _p3m9x2m2x * _1pyt1pz, + dN(15,1) = _1mx2t1p3x * _1pz, + dN(15,2) = _1mx2t1p3x * _1py; + + btScalar _m3m9y2m2y = -_3m9y2 - _2y; + btScalar _p3m9y2m2y = _3m9y2 - _2y; + btScalar _1my2t1m3y = _1my2 * _1m3y; + btScalar _1my2t1p3y = _1my2 * _1p3y; + dN(16,0) = -_1my2t1m3y * _1mz, + dN(16,1) = _m3m9y2m2y * _1mxt1mz, + dN(16,2) = -_1my2t1m3y * _1mx; + dN(17,0) = -_1my2t1p3y * _1mz, + dN(17,1) = _p3m9y2m2y * _1mxt1mz, + dN(17,2) = -_1my2t1p3y * _1mx; + dN(18,0) = _1my2t1m3y * _1mz, + dN(18,1) = _m3m9y2m2y * _1pxt1mz, + dN(18,2) = -_1my2t1m3y * _1px; + dN(19,0) = _1my2t1p3y * _1mz, + dN(19,1) = _p3m9y2m2y * _1pxt1mz, + dN(19,2) = -_1my2t1p3y * _1px; + dN(20,0) = -_1my2t1m3y * _1pz, + dN(20,1) = _m3m9y2m2y * _1mxt1pz, + dN(20,2) = _1my2t1m3y * _1mx; + dN(21,0) = -_1my2t1p3y * _1pz, + dN(21,1) = _p3m9y2m2y * _1mxt1pz, + dN(21,2) = _1my2t1p3y * _1mx; + dN(22,0) = _1my2t1m3y * _1pz, + dN(22,1) = _m3m9y2m2y * _1pxt1pz, + dN(22,2) = _1my2t1m3y * _1px; + dN(23,0) = _1my2t1p3y * _1pz, + dN(23,1) = _p3m9y2m2y * _1pxt1pz, + dN(23,2) = _1my2t1p3y * _1px; + + + btScalar _m3m9z2m2z = -_3m9z2 - _2z; + btScalar _p3m9z2m2z = _3m9z2 - _2z; + btScalar _1mz2t1m3z = _1mz2 * _1m3z; + btScalar _1mz2t1p3z = _1mz2 * _1p3z; + dN(24,0) = -_1mz2t1m3z * _1my, + dN(24,1) = -_1mz2t1m3z * _1mx, + dN(24,2) = _m3m9z2m2z * _1mxt1my; + dN(25,0) = -_1mz2t1p3z * _1my, + dN(25,1) = -_1mz2t1p3z * _1mx, + dN(25,2) = _p3m9z2m2z * _1mxt1my; + dN(26,0) = -_1mz2t1m3z * _1py, + dN(26,1) = _1mz2t1m3z * _1mx, + dN(26,2) = _m3m9z2m2z * _1mxt1py; + dN(27,0) = -_1mz2t1p3z * _1py, + dN(27,1) = _1mz2t1p3z * _1mx, + dN(27,2) = _p3m9z2m2z * _1mxt1py; + dN(28,0) = _1mz2t1m3z * _1my, + dN(28,1) = -_1mz2t1m3z * _1px, + dN(28,2) = _m3m9z2m2z * _1pxt1my; + dN(29,0) = _1mz2t1p3z * _1my, + dN(29,1) = -_1mz2t1p3z * _1px, + dN(29,2) = _p3m9z2m2z * _1pxt1my; + dN(30,0) = _1mz2t1m3z * _1py, + dN(30,1) = _1mz2t1m3z * _1px, + dN(30,2) = _m3m9z2m2z * _1pxt1py; + dN(31,0) = _1mz2t1p3z * _1py, + dN(31,1) = _1mz2t1p3z * _1px, + dN(31,2) = _p3m9z2m2z * _1pxt1py; + + dN.bottomRowsMul(32u - 8u, 9.0 / 64.0); + + } + + return res; +} + + + +bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x, + btVector3* gradient) const +{ + btAssert(m_isValid); + if (!m_isValid) + return false; + + if (!m_domain.contains(x)) + return false; + + btVector3 tmpmi = ((x - m_domain.min())*(m_inv_cell_size));//.cast<unsigned int>().eval(); + unsigned int mi[3] = {(unsigned int )tmpmi[0],(unsigned int )tmpmi[1],(unsigned int )tmpmi[2]}; + if (mi[0] >= m_resolution[0]) + mi[0] = m_resolution[0]-1; + if (mi[1] >= m_resolution[1]) + mi[1] = m_resolution[1]-1; + if (mi[2] >= m_resolution[2]) + mi[2] = m_resolution[2]-1; + btMultiIndex mui; + mui.ijk[0] = mi[0]; + mui.ijk[1] = mi[1]; + mui.ijk[2] = mi[2]; + int i = multiToSingleIndex(mui); + unsigned int i_ = m_cell_map[field_id][i]; + if (i_ == UINT_MAX) + return false; + + btAlignedBox3d sd = subdomain(i); + i = i_; + btVector3 d = sd.m_max-sd.m_min;//.diagonal().eval(); + + btVector3 denom = (sd.max() - sd.min()); + btVector3 c0 = btVector3(2.0,2.0,2.0)/denom; + btVector3 c1 = (sd.max() + sd.min())/denom; + btVector3 xi = (c0*x - c1); + + btCell32 const& cell = m_cells[field_id][i]; + if (!gradient) + { + //auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0)); + double phi = 0.0; + btShapeMatrix N = shape_function_(xi, 0); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + return false;; + } + phi += c * N[j]; + } + + dist = phi; + return true; + } + + btShapeGradients dN; + btShapeMatrix N = shape_function_(xi, &dN); + + double phi = 0.0; + gradient->setZero(); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + gradient->setZero(); + return false; + } + phi += c * N[j]; + (*gradient)[0] += c * dN(j, 0); + (*gradient)[1] += c * dN(j, 1); + (*gradient)[2] += c * dN(j, 2); + } + (*gradient) *= c0; + dist = phi; + return true; +} + |