diff options
author | Sebastian Parborg <darkdefende@gmail.com> | 2020-09-02 21:41:30 +0300 |
---|---|---|
committer | Sebastian Parborg <darkdefende@gmail.com> | 2020-09-02 21:41:30 +0300 |
commit | 4446c3a593c51603e135e38951607b9b668ddec5 (patch) | |
tree | 9552dbd903b4fd05ea740e1bba9b1b87d97414a1 /extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp | |
parent | 6f6f6ee18695dad66ad8aa0eb2bcab72501df597 (diff) |
Sync Bullet to upstream
This syncs Bullet to the latest upstream git version as of writing this.
(commit 47b0259b9700455022b5cf79b651cc1dc71dd59e).
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp')
-rw-r--r-- | extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp | 266 |
1 files changed, 153 insertions, 113 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 912a5285568..e50f85e2bbe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConvexConcaveCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -26,11 +26,12 @@ 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), -m_isSwapped(isSwapped), -m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) +btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped), + m_isSwapped(isSwapped) { } @@ -38,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() { } -void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_btConvexTriangleCallback.m_manifoldPtr) { @@ -46,55 +47,46 @@ void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& } } - -btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): - m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) +btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) { - m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap; - m_triBodyWrap = isSwapped? body0Wrap:body1Wrap; - - // - // create the manifold from the dispatcher 'manifold pool' - // - m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject()); + m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap; + m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap; + + // + // create the manifold from the dispatcher 'manifold pool' + // + m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject()); - clearCache(); + clearCache(); } btConvexTriangleCallback::~btConvexTriangleCallback() { clearCache(); - m_dispatcher->releaseManifold( m_manifoldPtr ); - + m_dispatcher->releaseManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::clearCache() +void btConvexTriangleCallback::clearCache() { m_dispatcher->clearManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::processTriangle(btVector3* triangle,int -partId, int triangleIndex) +void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("btConvexTriangleCallback::processTriangle"); if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) { return; } - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - - + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - - #if 0 ///debug drawing of the overlapping triangles @@ -108,52 +100,55 @@ partId, int triangleIndex) m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); } #endif - + if (m_convexBodyWrap->getCollisionShape()->isConvex()) { - btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + btTriangleShape tm(triangle[0], triangle[1], triangle[2]); tm.setMargin(m_collisionMarginTriangle); - - - btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr); + btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform? + btCollisionAlgorithm* colAlgo = 0; + + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS); + } const btCollisionObjectWrapper* tmpWrap = 0; if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { tmpWrap = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersA(partId,triangleIndex); + m_resultOut->setShapeIdentifiersA(partId, triangleIndex); } else { tmpWrap = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersB(partId,triangleIndex); + m_resultOut->setShapeIdentifiersB(partId, triangleIndex); } - - colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut); + + colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut); if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - - colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); } - } - - -void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) { m_convexBodyWrap = convexBodyWrap; m_triBodyWrap = triBodyWrap; @@ -167,65 +162,120 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape()); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle; - btVector3 extra(extraMargin,extraMargin,extraMargin); + convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax); + btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold; + + btVector3 extra(extraMargin, extraMargin, extraMargin); m_aabbMax += extra; m_aabbMin -= extra; - } void btConvexConcaveCollisionAlgorithm::clearCache() { m_btConvexTriangleCallback.clearCache(); - } -void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - - + BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); + const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; if (triBodyWrap->getCollisionShape()->isConcave()) { + if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) + { + 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); + } + } + btScalar maxDist = SIMD_EPSILON; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + queryVertices.push_back(btVector3(0, 0, 0)); + btSphereShape* sphere = (btSphereShape*)convex; + maxDist = sphere->getRadius() + SIMD_EPSILON; + } + if (queryVertices.size()) + { + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); + + 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); + + btVector3 normalLocal; + btScalar dist; + if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal)) + { + if (dist <= maxDist) + { + normalLocal.safeNormalize(); + btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + btSphereShape* sphere = (btSphereShape*)convex; + dist -= sphere->getRadius(); + vtxWorldSpace -= sphere->getRadius() * normal; + } + 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(); - - 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); + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut); - m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject()); - concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); - - resultOut->refreshContactPoints(); + concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax()); - m_btConvexTriangleCallback.clearWrapperData(); - + resultOut->refreshContactPoints(); + + m_btConvexTriangleCallback.clearWrapperData(); + } } - } - } - -btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) //only perform CCD above a certain threshold, this prevents blocking on the long run @@ -244,27 +294,26 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - struct LocalTriangleSphereCastCallback : public btTriangleCallback + struct LocalTriangleSphereCastCallback : public btTriangleCallback { btTransform m_ccdSphereFromTrans; btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; + btTransform m_meshTransform; - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { + LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction) + : m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { } - - + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("processTriangle"); (void)partId; (void)triangleIndex; //do a swept sphere for now @@ -272,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj ident.setIdentity(); btConvexCast::CastResult castResult; castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + btSphereShape pointShape(m_ccdSphereRadius); + btTriangleShape triShape(triangle[0], triangle[1], triangle[2]); + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); //local space? - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, + ident, ident, castResult)) { if (m_hitFraction > castResult.m_fraction) m_hitFraction = castResult.m_fraction; } - } - }; - - - - if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -302,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); + btScalar curHitFraction = btScalar(1.); //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal, + convexbody->getCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody->getHitFraction(); btCollisionObject* concavebody = triBody; - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - + btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape(); + if (triangleMesh) { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax); } - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) { - convexbody->setHitFraction( raycastCallback.m_hitFraction); + convexbody->setHitFraction(raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } return btScalar(1.); - } |