diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp | 198 |
1 files changed, 78 insertions, 120 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index ab84bddf2a..750718f57f 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSoftBodyConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" @@ -27,34 +26,28 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexHullShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletSoftBody/btSoftBody.h" -#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable +#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06) //make this configurable -btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -: btCollisionAlgorithm(ci), -m_isSwapped(isSwapped), -m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) +btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btCollisionAlgorithm(ci), + m_isSwapped(isSwapped), + m_btSoftBodyTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped) { } - - btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() { } - - -btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): -m_dispatcher(dispatcher), -m_dispatchInfoPtr(0) +btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) { - m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject()); - m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject(); + m_softBody = (isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject()); + m_triBody = isSwapped ? body0Wrap->getCollisionObject() : body1Wrap->getCollisionObject(); // // create the manifold from the dispatcher 'manifold pool' @@ -68,46 +61,42 @@ btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() { clearCache(); // m_dispatcher->releaseManifold( m_manifoldPtr ); - } - -void btSoftBodyTriangleCallback::clearCache() +void btSoftBodyTriangleCallback::clearCache() { - for (int i=0;i<m_shapeCache.size();i++) + for (int i = 0; i < m_shapeCache.size(); i++) { btTriIndex* tmp = m_shapeCache.getAtIndex(i); btAssert(tmp); btAssert(tmp->m_childShape); - m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary? + m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape); //necessary? delete tmp->m_childShape; } m_shapeCache.clear(); } - -void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { //just for debugging purposes //printf("triangle %d",m_triangleCount++); - + btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe)) + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { - btVector3 color(1,1,0); + btVector3 color(1, 1, 0); const btTransform& tr = m_triBody->getWorldTransform(); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); } - btTriIndex triIndex(partId,triangleIndex,0); + btTriIndex triIndex(partId, triangleIndex, 0); btHashKey<btTriIndex> triKey(triIndex.getUid()); - btTriIndex* shapeIndex = m_shapeCache[triKey]; if (shapeIndex) { @@ -117,82 +106,73 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //copy over user pointers to temporary shape tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer()); - btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); + btCollisionObjectWrapper softBody(0, m_softBody->getCollisionShape(), m_softBody, m_softBody->getWorldTransform(), -1, -1); //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//?? - btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex); + btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex); ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType); //m_manifoldPtr); - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - + return; } - //aabb filter is already applied! + //aabb filter is already applied! //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); // if (m_softBody->getCollisionShape()->getShapeType()== { // btVector3 other; - btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); + btVector3 normal = (triangle[1] - triangle[0]).cross(triangle[2] - triangle[0]); normal.normalize(); - normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; + normal *= BT_SOFTBODY_TRIANGLE_EXTRUSION; // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; // other+=normal*22.f; - btVector3 pts[6] = {triangle[0]+normal, - triangle[1]+normal, - triangle[2]+normal, - triangle[0]-normal, - triangle[1]-normal, - triangle[2]-normal}; - - btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); + btVector3 pts[6] = {triangle[0] + normal, + triangle[1] + normal, + triangle[2] + normal, + triangle[0] - normal, + triangle[1] - normal, + triangle[2] - normal}; + btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(), 6); // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); - //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); // tm.setMargin(m_collisionMarginTriangle); //copy over user pointers to temporary shape tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer()); - - btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); - btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//?? + btCollisionObjectWrapper softBody(0, m_softBody->getCollisionShape(), m_softBody, m_softBody->getWorldTransform(), -1, -1); + btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex); //btTransform::getIdentity());//?? ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType); //m_manifoldPtr); - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); triIndex.m_childShape = tm; - m_shapeCache.insert(triKey,triIndex); - + m_shapeCache.insert(triKey, triIndex); } - - - } - - -void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { m_dispatchInfoPtr = &dispatchInfo; - m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); + m_collisionMarginTriangle = collisionMarginTriangle + btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); m_resultOut = resultOut; - - btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; - m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); - btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); - btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5); + btVector3 aabbWorldSpaceMin, aabbWorldSpaceMax; + m_softBody->getAabb(aabbWorldSpaceMin, aabbWorldSpaceMax); + btVector3 halfExtents = (aabbWorldSpaceMax - aabbWorldSpaceMin) * btScalar(0.5); + btVector3 softBodyCenter = (aabbWorldSpaceMax + aabbWorldSpaceMin) * btScalar(0.5); btTransform softTransform; softTransform.setIdentity(); @@ -200,56 +180,45 @@ void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMargin btTransform convexInTriangleSpace; convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform; - btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); + btTransformAabb(halfExtents, m_collisionMarginTriangle, convexInTriangleSpace, m_aabbMin, m_aabbMax); } void btSoftBodyConcaveCollisionAlgorithm::clearCache() { m_btSoftBodyTriangleCallback.clearCache(); - } -void btSoftBodyConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSoftBodyConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - - //btCollisionObject* convexBody = m_isSwapped ? body1 : body0; const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap; if (triBody->getCollisionShape()->isConcave()) { - - - const btCollisionObject* triOb = triBody->getCollisionObject(); - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triOb->getCollisionShape()); + const btCollisionObject* triOb = triBody->getCollisionObject(); + const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triOb->getCollisionShape()); // if (convexBody->getCollisionShape()->isConvex()) { btScalar collisionMarginTriangle = concaveShape->getMargin(); // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); - m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut); + m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, triBody, dispatchInfo, resultOut); - - concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); + concaveShape->processAllTriangles(&m_btSoftBodyTriangleCallback, m_btSoftBodyTriangleCallback.getAabbMin(), m_btSoftBodyTriangleCallback.getAabbMax()); // resultOut->refreshContactPoints(); - } - } - } - -btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSoftBodyConcaveCollisionAlgorithm::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 @@ -268,25 +237,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO 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) { (void)partId; @@ -296,29 +263,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO 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(); @@ -326,33 +287,30 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO 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.); - } |