diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index 868d25529..a4f4b9b3a 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -185,7 +185,13 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans } } else { - btAssert(0); + if (collisionShape->getShapeType()==SDF_SHAPE_PROXYTYPE) + { + //not yet + } else + { + btAssert(0); + } } } diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index adaf083a2..f6e1202a6 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -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, diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 39ff7934d..d2331f5df 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -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( 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 queryVertices; + + if (convex->isPolyhedral()) + { + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*) convex; + for (int v=0;vgetNumVertices();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;vgetWorldTransform()*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( 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(); + } } } diff --git a/src/BulletCollision/CollisionShapes/btMiniSDF.cpp b/src/BulletCollision/CollisionShapes/btMiniSDF.cpp new file mode 100644 index 000000000..f11daee12 --- /dev/null +++ b/src/BulletCollision/CollisionShapes/btMiniSDF.cpp @@ -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 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& nodes = m_nodes[i]; + nodes.resize(n_nodes1); + for ( int j=0;j& cells = m_cells[i]; + ds.read(n_cells1); + cells.resize(n_cells1); + for (int j=0;j& cell_maps = m_cell_map[i]; + ds.read(n_cell_maps1); + cell_maps.resize(n_cell_maps1); + for (int j=0;j().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; +} + diff --git a/src/BulletCollision/CollisionShapes/btMiniSDF.h b/src/BulletCollision/CollisionShapes/btMiniSDF.h new file mode 100644 index 000000000..221b3494e --- /dev/null +++ b/src/BulletCollision/CollisionShapes/btMiniSDF.h @@ -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 > m_nodes; + btAlignedObjectArray > m_cells; + btAlignedObjectArray > 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 diff --git a/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp new file mode 100644 index 000000000..828acda47 --- /dev/null +++ b/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -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; +} diff --git a/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h new file mode 100644 index 000000000..6e32db9cd --- /dev/null +++ b/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h @@ -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