mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-18 21:10:05 +00:00
Preparation to add signed distance field collision detection.
btMiniSDF is based on https://github.com/InteractiveComputerGraphics/Discregrid
This commit is contained in:
erwincoumans
parent
380e59be6a
commit
698836d54e
@ -185,7 +185,13 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
|
||||
}
|
||||
} else
|
||||
{
|
||||
btAssert(0);
|
||||
if (collisionShape->getShapeType()==SDF_SHAPE_PROXYTYPE)
|
||||
{
|
||||
//not yet
|
||||
} else
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -66,6 +66,7 @@ CONCAVE_SHAPES_START_HERE,
|
||||
EMPTY_SHAPE_PROXYTYPE,
|
||||
STATIC_PLANE_PROXYTYPE,
|
||||
CUSTOM_CONCAVE_SHAPE_TYPE,
|
||||
SDF_SHAPE_PROXYTYPE=CUSTOM_CONCAVE_SHAPE_TYPE,
|
||||
CONCAVE_SHAPES_END_HERE,
|
||||
|
||||
COMPOUND_SHAPE_PROXYTYPE,
|
||||
|
||||
@ -27,6 +27,7 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
|
||||
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
|
||||
|
||||
btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
|
||||
: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
|
||||
@ -202,26 +203,78 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
|
||||
|
||||
if (triBodyWrap->getCollisionShape()->isConcave())
|
||||
{
|
||||
|
||||
|
||||
|
||||
const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
|
||||
|
||||
if (convexBodyWrap->getCollisionShape()->isConvex())
|
||||
if (triBodyWrap->getCollisionShape()->getShapeType()==SDF_SHAPE_PROXYTYPE)
|
||||
{
|
||||
btScalar collisionMarginTriangle = concaveShape->getMargin();
|
||||
btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
|
||||
if (convexBodyWrap->getCollisionShape()->isConvex())
|
||||
{
|
||||
|
||||
btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
|
||||
btAlignedObjectArray<btVector3> queryVertices;
|
||||
|
||||
if (convex->isPolyhedral())
|
||||
{
|
||||
btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*) convex;
|
||||
for (int v=0;v<poly->getNumVertices();v++)
|
||||
{
|
||||
btVector3 vtx;
|
||||
poly->getVertex(v,vtx);
|
||||
queryVertices.push_back(vtx);
|
||||
}
|
||||
}
|
||||
if (convex->getShapeType()==SPHERE_SHAPE_PROXYTYPE)
|
||||
{
|
||||
queryVertices.push_back(btVector3(0,0,0));
|
||||
|
||||
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
|
||||
m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
|
||||
}
|
||||
if (queryVertices.size())
|
||||
{
|
||||
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
|
||||
//m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
|
||||
|
||||
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
|
||||
btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*) convex;
|
||||
for (int v=0;v<queryVertices.size();v++)
|
||||
{
|
||||
const btVector3& vtx = queryVertices[v];
|
||||
btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx;
|
||||
btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
|
||||
|
||||
concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
|
||||
btVector3 normalLocal;
|
||||
btScalar dist;
|
||||
if (sdfShape->queryPoint(vtxInSdf,dist, normalLocal))
|
||||
{
|
||||
if (dist<=SIMD_EPSILON)
|
||||
{
|
||||
normalLocal.safeNormalize();
|
||||
btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal;
|
||||
resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist);
|
||||
}
|
||||
}
|
||||
}
|
||||
resultOut->refreshContactPoints();
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
|
||||
|
||||
if (convexBodyWrap->getCollisionShape()->isConvex())
|
||||
{
|
||||
btScalar collisionMarginTriangle = concaveShape->getMargin();
|
||||
|
||||
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
|
||||
m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
|
||||
|
||||
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
|
||||
|
||||
concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
|
||||
|
||||
resultOut->refreshContactPoints();
|
||||
resultOut->refreshContactPoints();
|
||||
|
||||
m_btConvexTriangleCallback.clearWrapperData();
|
||||
m_btConvexTriangleCallback.clearWrapperData();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
521
src/BulletCollision/CollisionShapes/btMiniSDF.cpp
Normal file
521
src/BulletCollision/CollisionShapes/btMiniSDF.cpp
Normal file
@ -0,0 +1,521 @@
|
||||
#include "btMiniSDF.h"
|
||||
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
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;
|
||||
|
||||
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, nullptr));
|
||||
double phi = 0.0;
|
||||
btShapeMatrix N = shape_function_(xi, nullptr);
|
||||
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;
|
||||
}
|
||||
|
||||
134
src/BulletCollision/CollisionShapes/btMiniSDF.h
Normal file
134
src/BulletCollision/CollisionShapes/btMiniSDF.h
Normal file
@ -0,0 +1,134 @@
|
||||
#ifndef MINISDF_H
|
||||
#define MINISDF_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btAabbUtil2.h"
|
||||
#include "Linearmath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
struct btMultiIndex
|
||||
{
|
||||
unsigned int ijk[3];
|
||||
};
|
||||
|
||||
struct btAlignedBox3d
|
||||
{
|
||||
btVector3 m_min;
|
||||
btVector3 m_max;
|
||||
|
||||
const btVector3& min() const
|
||||
{
|
||||
return m_min;
|
||||
}
|
||||
|
||||
const btVector3& max() const
|
||||
{
|
||||
return m_max;
|
||||
}
|
||||
|
||||
|
||||
bool contains(const btVector3& x) const
|
||||
{
|
||||
return TestPointAgainstAabb2(m_min, m_max, x);
|
||||
}
|
||||
|
||||
btAlignedBox3d(const btVector3& mn, const btVector3& mx)
|
||||
:m_min(mn),
|
||||
m_max(mx)
|
||||
{
|
||||
}
|
||||
|
||||
btAlignedBox3d()
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct btShapeMatrix
|
||||
{
|
||||
double m_vec[32];
|
||||
|
||||
inline double& operator[](int i)
|
||||
{
|
||||
return m_vec[i];
|
||||
}
|
||||
|
||||
inline const double& operator[](int i) const
|
||||
{
|
||||
return m_vec[i];
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
struct btShapeGradients
|
||||
{
|
||||
btVector3 m_vec[32];
|
||||
|
||||
void topRowsDivide(int row, double denom)
|
||||
{
|
||||
for (int i=0;i<row;i++)
|
||||
{
|
||||
m_vec[i] /= denom;
|
||||
}
|
||||
}
|
||||
|
||||
void bottomRowsMul(int row, double val)
|
||||
{
|
||||
for (int i=32-row;i<32;i++)
|
||||
{
|
||||
m_vec[i] *= val;
|
||||
}
|
||||
}
|
||||
|
||||
inline double& operator()(int i, int j)
|
||||
{
|
||||
return m_vec[i][j];
|
||||
}
|
||||
};
|
||||
|
||||
struct btCell32
|
||||
{
|
||||
unsigned int m_cells[32];
|
||||
};
|
||||
|
||||
struct btMiniSDF
|
||||
{
|
||||
|
||||
btAlignedBox3d m_domain;
|
||||
unsigned int m_resolution[3];
|
||||
btVector3 m_cell_size;
|
||||
btVector3 m_inv_cell_size;
|
||||
std::size_t m_n_cells;
|
||||
std::size_t m_n_fields;
|
||||
bool m_isValid;
|
||||
|
||||
|
||||
btAlignedObjectArray<btAlignedObjectArray<double> > m_nodes;
|
||||
btAlignedObjectArray<btAlignedObjectArray<btCell32 > > m_cells;
|
||||
btAlignedObjectArray<btAlignedObjectArray<unsigned int> > m_cell_map;
|
||||
|
||||
btMiniSDF()
|
||||
:m_isValid(false)
|
||||
{
|
||||
}
|
||||
bool load(const char* data, int size);
|
||||
bool isValid() const
|
||||
{
|
||||
return m_isValid;
|
||||
}
|
||||
unsigned int multiToSingleIndex(btMultiIndex const & ijk) const;
|
||||
|
||||
btAlignedBox3d subdomain(btMultiIndex const& ijk) const;
|
||||
|
||||
btMultiIndex singleToMultiIndex(unsigned int l) const;
|
||||
|
||||
btAlignedBox3d subdomain(unsigned int l) const;
|
||||
|
||||
|
||||
btShapeMatrix
|
||||
shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const;
|
||||
|
||||
bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const;
|
||||
};
|
||||
|
||||
|
||||
#endif //MINISDF_H
|
||||
99
src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
Normal file
99
src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
Normal file
@ -0,0 +1,99 @@
|
||||
#include "btSdfCollisionShape.h"
|
||||
#include "btMiniSDF.h"
|
||||
#include "LinearMath/btAabbUtil2.h"
|
||||
|
||||
struct btSdfCollisionShapeInternalData
|
||||
{
|
||||
btVector3 m_localScaling;
|
||||
btScalar m_margin;
|
||||
btMiniSDF m_sdf;
|
||||
|
||||
btSdfCollisionShapeInternalData()
|
||||
:m_localScaling(1,1,1),
|
||||
m_margin(0)
|
||||
{
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes)
|
||||
{
|
||||
bool valid = m_data->m_sdf.load(sdfData, sizeInBytes);
|
||||
return valid;
|
||||
}
|
||||
btSdfCollisionShape::btSdfCollisionShape()
|
||||
{
|
||||
m_shapeType = SDF_SHAPE_PROXYTYPE;
|
||||
m_data = new btSdfCollisionShapeInternalData();
|
||||
|
||||
|
||||
|
||||
//"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf");
|
||||
/*unsigned int field_id=0;
|
||||
Eigen::Vector3d x (1,10,1);
|
||||
Eigen::Vector3d gradient;
|
||||
double dist = m_data->m_sdf.interpolate(field_id, x, &gradient);
|
||||
printf("dist=%g\n", dist);
|
||||
*/
|
||||
|
||||
}
|
||||
btSdfCollisionShape::~btSdfCollisionShape()
|
||||
{
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void btSdfCollisionShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
|
||||
{
|
||||
btAssert(m_data->m_sdf.isValid());
|
||||
btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min;
|
||||
btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max;
|
||||
btScalar margin(0);
|
||||
btTransformAabb(localAabbMin,localAabbMax,margin,t,aabbMin,aabbMax);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btSdfCollisionShape::setLocalScaling(const btVector3& scaling)
|
||||
{
|
||||
m_data->m_localScaling = scaling;
|
||||
}
|
||||
const btVector3& btSdfCollisionShape::getLocalScaling() const
|
||||
{
|
||||
return m_data->m_localScaling;
|
||||
}
|
||||
void btSdfCollisionShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
|
||||
{
|
||||
inertia.setValue(0,0,0);
|
||||
}
|
||||
const char* btSdfCollisionShape::getName()const
|
||||
{
|
||||
return "btSdfCollisionShape";
|
||||
}
|
||||
void btSdfCollisionShape::setMargin(btScalar margin)
|
||||
{
|
||||
m_data->m_margin = margin;
|
||||
}
|
||||
btScalar btSdfCollisionShape::getMargin() const
|
||||
{
|
||||
return m_data->m_margin;
|
||||
}
|
||||
|
||||
void btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
|
||||
{
|
||||
//not yet
|
||||
}
|
||||
|
||||
|
||||
bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal)
|
||||
{
|
||||
int field = 0;
|
||||
btVector3 grad;
|
||||
double dist;
|
||||
bool hasResult = m_data->m_sdf.interpolate(field,dist, ptInSDF,&grad);
|
||||
if (hasResult)
|
||||
{
|
||||
normal.setValue(grad[0],grad[1],grad[2]);
|
||||
distOut= dist;
|
||||
}
|
||||
return hasResult;
|
||||
}
|
||||
30
src/BulletCollision/CollisionShapes/btSdfCollisionShape.h
Normal file
30
src/BulletCollision/CollisionShapes/btSdfCollisionShape.h
Normal file
@ -0,0 +1,30 @@
|
||||
#ifndef BT_SDF_COLLISION_SHAPE_H
|
||||
#define BT_SDF_COLLISION_SHAPE_H
|
||||
|
||||
#include "btConcaveShape.h"
|
||||
|
||||
class btSdfCollisionShape : public btConcaveShape
|
||||
{
|
||||
struct btSdfCollisionShapeInternalData* m_data;
|
||||
|
||||
public:
|
||||
|
||||
btSdfCollisionShape();
|
||||
virtual ~btSdfCollisionShape();
|
||||
|
||||
bool initializeSDF(const char* sdfData, int sizeInBytes);
|
||||
|
||||
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
|
||||
virtual void setLocalScaling(const btVector3& scaling);
|
||||
virtual const btVector3& getLocalScaling() const;
|
||||
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
|
||||
virtual const char* getName()const;
|
||||
virtual void setMargin(btScalar margin);
|
||||
virtual btScalar getMargin() const;
|
||||
|
||||
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
|
||||
|
||||
bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal);
|
||||
};
|
||||
|
||||
#endif //BT_SDF_COLLISION_SHAPE_H
|
||||
Loading…
Reference in New Issue
Block a user