| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| |
| #include "BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionObject.h" |
| #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" |
| #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" |
| #include "BulletCollision/CollisionShapes/btConcaveShape.h" |
| #include "BulletCollision/CollisionDispatch/btManifoldResult.h" |
| #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" |
| #include "BulletCollision/CollisionShapes/btTriangleShape.h" |
| #include "BulletCollision/CollisionShapes/btSphereShape.h" |
| #include "BulletCollision/CollisionShapes/btTetrahedronShape.h" |
| #include "BulletCollision/CollisionShapes/btConvexHullShape.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 |
| |
| btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) |
| : btCollisionAlgorithm(ci), |
| m_isSwapped(isSwapped), |
| m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) |
| { |
| } |
| |
| |
| |
| btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() |
| { |
| } |
| |
| |
| |
| btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): |
| m_dispatcher(dispatcher), |
| m_dispatchInfoPtr(0) |
| { |
| m_softBody = (btSoftBody*) (isSwapped? body1:body0); |
| m_triBody = isSwapped? body0:body1; |
| |
| // |
| // create the manifold from the dispatcher 'manifold pool' |
| // |
| // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); |
| |
| clearCache(); |
| } |
| |
| btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() |
| { |
| clearCache(); |
| // m_dispatcher->releaseManifold( m_manifoldPtr ); |
| |
| } |
| |
| |
| void btSoftBodyTriangleCallback::clearCache() |
| { |
| 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? |
| delete tmp->m_childShape; |
| } |
| m_shapeCache.clear(); |
| } |
| |
| |
| void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) |
| { |
| //just for debugging purposes |
| //printf("triangle %d",m_triangleCount++); |
| btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); |
| 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) |
| { |
| btVector3 color(255,255,0); |
| btTransform& tr = ob->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); |
| } |
| |
| btTriIndex triIndex(partId,triangleIndex,0); |
| btHashKey<btTriIndex> triKey(triIndex.getUid()); |
| |
| |
| btTriIndex* shapeIndex = m_shapeCache[triKey]; |
| if (shapeIndex) |
| { |
| btCollisionShape* tm = shapeIndex->m_childShape; |
| btAssert(tm); |
| |
| //copy over user pointers to temporary shape |
| tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); |
| |
| btCollisionShape* tmpShape = ob->getCollisionShape(); |
| ob->internalSetTemporaryCollisionShape( tm ); |
| |
| |
| btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); |
| |
| colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); |
| colAlgo->~btCollisionAlgorithm(); |
| ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); |
| ob->internalSetTemporaryCollisionShape( tmpShape); |
| return; |
| } |
| |
| //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]); |
| normal.normalize(); |
| 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); |
| |
| |
| // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); |
| |
| //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); |
| // tm.setMargin(m_collisionMarginTriangle); |
| |
| //copy over user pointers to temporary shape |
| tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); |
| |
| btCollisionShape* tmpShape = ob->getCollisionShape(); |
| ob->internalSetTemporaryCollisionShape( tm ); |
| |
| |
| btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); |
| ///this should use the btDispatcher, so the actual registered algorithm is used |
| // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); |
| |
| //m_resultOut->setShapeIdentifiersB(partId,triangleIndex); |
| // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); |
| colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); |
| colAlgo->~btCollisionAlgorithm(); |
| ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); |
| |
| |
| ob->internalSetTemporaryCollisionShape( tmpShape ); |
| triIndex.m_childShape = tm; |
| m_shapeCache.insert(triKey,triIndex); |
| |
| } |
| |
| |
| |
| } |
| |
| |
| |
| void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) |
| { |
| m_dispatchInfoPtr = &dispatchInfo; |
| 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); |
| |
| btTransform softTransform; |
| softTransform.setIdentity(); |
| softTransform.setOrigin(softBodyCenter); |
| |
| btTransform convexInTriangleSpace; |
| convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform; |
| btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); |
| } |
| |
| void btSoftBodyConcaveCollisionAlgorithm::clearCache() |
| { |
| m_btSoftBodyTriangleCallback.clearCache(); |
| |
| } |
| |
| void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) |
| { |
| |
| |
| //btCollisionObject* convexBody = m_isSwapped ? body1 : body0; |
| btCollisionObject* triBody = m_isSwapped ? body0 : body1; |
| |
| if (triBody->getCollisionShape()->isConcave()) |
| { |
| |
| |
| btCollisionObject* triOb = triBody; |
| btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape()); |
| |
| // if (convexBody->getCollisionShape()->isConvex()) |
| { |
| btScalar collisionMarginTriangle = concaveShape->getMargin(); |
| |
| // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); |
| m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); |
| |
| //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. |
| //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); |
| |
| // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); |
| |
| |
| concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); |
| |
| // resultOut->refreshContactPoints(); |
| |
| } |
| |
| } |
| |
| } |
| |
| |
| 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 |
| //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... |
| btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); |
| if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) |
| { |
| return btScalar(1.); |
| } |
| |
| //const btVector3& from = convexbody->m_worldTransform.getOrigin(); |
| //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); |
| //todo: only do if the motion exceeds the 'radius' |
| |
| btTransform triInv = triBody->getWorldTransform().inverse(); |
| btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); |
| btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); |
| |
| struct LocalTriangleSphereCastCallback : public btTriangleCallback |
| { |
| btTransform m_ccdSphereFromTrans; |
| btTransform m_ccdSphereToTrans; |
| btTransform m_meshTransform; |
| |
| 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) |
| { |
| } |
| |
| |
| virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) |
| { |
| (void)partId; |
| (void)triangleIndex; |
| //do a swept sphere for now |
| btTransform ident; |
| 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); |
| //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); |
| //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); |
| //local space? |
| |
| 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(); |
| rayAabbMin.setMin(convexToLocal.getOrigin()); |
| btVector3 rayAabbMax = convexFromLocal.getOrigin(); |
| rayAabbMax.setMax(convexToLocal.getOrigin()); |
| btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); |
| rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); |
| rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); |
| |
| 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(); |
| |
| if (triangleMesh) |
| { |
| triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); |
| } |
| |
| |
| |
| if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) |
| { |
| convexbody->setHitFraction( raycastCallback.m_hitFraction); |
| return raycastCallback.m_hitFraction; |
| } |
| } |
| |
| return btScalar(1.); |
| |
| } |