| /* | |
| 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 "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.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 "LinearMath/btIDebugDraw.h" | |
| #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" | |
| btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) | |
| : btActivatingCollisionAlgorithm(ci,body0,body1), | |
| m_isSwapped(isSwapped), | |
| m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) | |
| { | |
| } | |
| btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() | |
| { | |
| } | |
| void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) | |
| { | |
| if (m_btConvexTriangleCallback.m_manifoldPtr) | |
| { | |
| manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr); | |
| } | |
| } | |
| btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): | |
| m_dispatcher(dispatcher), | |
| m_dispatchInfoPtr(0) | |
| { | |
| m_convexBody = 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(); | |
| } | |
| btConvexTriangleCallback::~btConvexTriangleCallback() | |
| { | |
| clearCache(); | |
| m_dispatcher->releaseManifold( m_manifoldPtr ); | |
| } | |
| void btConvexTriangleCallback::clearCache() | |
| { | |
| m_dispatcher->clearManifold(m_manifoldPtr); | |
| } | |
| void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) | |
| { | |
| //just for debugging purposes | |
| //printf("triangle %d",m_triangleCount++); | |
| //aabb filter is already applied! | |
| btCollisionAlgorithmConstructionInfo ci; | |
| ci.m_dispatcher1 = m_dispatcher; | |
| btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); | |
| ///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); | |
| //btVector3 center = triangle[0] + triangle[1]+triangle[2]; | |
| //center *= btScalar(0.333333); | |
| //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); | |
| //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); | |
| //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); | |
| } | |
| //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); | |
| if (m_convexBody->getCollisionShape()->isConvex()) | |
| { | |
| btTriangleShape tm(triangle[0],triangle[1],triangle[2]); | |
| tm.setMargin(m_collisionMarginTriangle); | |
| btCollisionShape* tmpShape = ob->getCollisionShape(); | |
| ob->internalSetTemporaryCollisionShape( &tm ); | |
| btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,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_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); | |
| colAlgo->~btCollisionAlgorithm(); | |
| ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); | |
| ob->internalSetTemporaryCollisionShape( tmpShape); | |
| } | |
| } | |
| void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) | |
| { | |
| m_dispatchInfoPtr = &dispatchInfo; | |
| m_collisionMarginTriangle = collisionMarginTriangle; | |
| m_resultOut = resultOut; | |
| //recalc aabbs | |
| btTransform convexInTriangleSpace; | |
| convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform(); | |
| btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape()); | |
| //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); | |
| convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); | |
| btScalar extraMargin = collisionMarginTriangle; | |
| btVector3 extra(extraMargin,extraMargin,extraMargin); | |
| m_aabbMax += extra; | |
| m_aabbMin -= extra; | |
| } | |
| void btConvexConcaveCollisionAlgorithm::clearCache() | |
| { | |
| m_btConvexTriangleCallback.clearCache(); | |
| } | |
| void btConvexConcaveCollisionAlgorithm::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_btConvexTriangleCallback.m_manifoldPtr); | |
| m_btConvexTriangleCallback.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_btConvexTriangleCallback.m_manifoldPtr); | |
| m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); | |
| concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); | |
| resultOut->refreshContactPoints(); | |
| } | |
| } | |
| } | |
| 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 | |
| //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.); | |
| } |