btConvexConcaveCollisionAlgorithm.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btConvexConcaveCollisionAlgorithm.h"
00018 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00019 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
00020 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
00021 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
00022 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
00023 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
00024 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00025 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00026 #include "LinearMath/btIDebugDraw.h"
00027 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00028 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00029 
00030 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
00031 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
00032 m_isSwapped(isSwapped),
00033 m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
00034 {
00035 }
00036 
00037 btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
00038 {
00039 }
00040 
00041 void    btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&      manifoldArray)
00042 {
00043         if (m_btConvexTriangleCallback.m_manifoldPtr)
00044         {
00045                 manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
00046         }
00047 }
00048 
00049 
00050 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
00051           m_dispatcher(dispatcher),
00052         m_dispatchInfoPtr(0)
00053 {
00054         m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
00055         m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
00056         
00057           //
00058           // create the manifold from the dispatcher 'manifold pool'
00059           //
00060           m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
00061 
00062           clearCache();
00063 }
00064 
00065 btConvexTriangleCallback::~btConvexTriangleCallback()
00066 {
00067         clearCache();
00068         m_dispatcher->releaseManifold( m_manifoldPtr );
00069   
00070 }
00071   
00072 
00073 void    btConvexTriangleCallback::clearCache()
00074 {
00075         m_dispatcher->clearManifold(m_manifoldPtr);
00076 }
00077 
00078 
00079 
00080 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
00081 {
00082  
00083         //just for debugging purposes
00084         //printf("triangle %d",m_triangleCount++);
00085 
00086 
00087         //aabb filter is already applied!       
00088 
00089         btCollisionAlgorithmConstructionInfo ci;
00090         ci.m_dispatcher1 = m_dispatcher;
00091 
00092         //const btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
00093 
00094 
00095 #if 0   
00096 
00097         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
00098         {
00099                 btVector3 color(1,1,0);
00100                 btTransform& tr = ob->getWorldTransform();
00101                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
00102                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
00103                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
00104         }
00105 #endif
00106         
00107         if (m_convexBodyWrap->getCollisionShape()->isConvex())
00108         {
00109                 btTriangleShape tm(triangle[0],triangle[1],triangle[2]);        
00110                 tm.setMargin(m_collisionMarginTriangle);
00111                 
00112                 
00113                 btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
00114                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
00115 
00116                 const btCollisionObjectWrapper* tmpWrap = 0;
00117 
00118                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
00119                 {
00120                         tmpWrap = m_resultOut->getBody0Wrap();
00121                         m_resultOut->setBody0Wrap(&triObWrap);
00122                         m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
00123                 }
00124                 else
00125                 {
00126                         tmpWrap = m_resultOut->getBody1Wrap();
00127                         m_resultOut->setBody1Wrap(&triObWrap);
00128                         m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
00129                 }
00130         
00131                 colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
00132 
00133                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
00134                 {
00135                         m_resultOut->setBody0Wrap(tmpWrap);
00136                 } else
00137                 {
00138                         m_resultOut->setBody1Wrap(tmpWrap);
00139                 }
00140                 
00141 
00142 
00143                 colAlgo->~btCollisionAlgorithm();
00144                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
00145         }
00146 
00147 }
00148 
00149 
00150 
00151 void    btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
00152 {
00153         m_convexBodyWrap = convexBodyWrap;
00154         m_triBodyWrap = triBodyWrap;
00155 
00156         m_dispatchInfoPtr = &dispatchInfo;
00157         m_collisionMarginTriangle = collisionMarginTriangle;
00158         m_resultOut = resultOut;
00159 
00160         //recalc aabbs
00161         btTransform convexInTriangleSpace;
00162         convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
00163         const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
00164         //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
00165         convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
00166         btScalar extraMargin = collisionMarginTriangle;
00167         btVector3 extra(extraMargin,extraMargin,extraMargin);
00168 
00169         m_aabbMax += extra;
00170         m_aabbMin -= extra;
00171         
00172 }
00173 
00174 void btConvexConcaveCollisionAlgorithm::clearCache()
00175 {
00176         m_btConvexTriangleCallback.clearCache();
00177 
00178 }
00179 
00180 void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00181 {
00182         
00183         
00184         const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
00185         const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
00186 
00187         if (triBodyWrap->getCollisionShape()->isConcave())
00188         {
00189 
00190 
00191                 
00192                 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
00193                 
00194                 if (convexBodyWrap->getCollisionShape()->isConvex())
00195                 {
00196                         btScalar collisionMarginTriangle = concaveShape->getMargin();
00197                                         
00198                         resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
00199                         m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
00200 
00201                         m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
00202 
00203                         concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
00204                         
00205                         resultOut->refreshContactPoints();
00206 
00207                         m_btConvexTriangleCallback.clearWrapperData();
00208         
00209                 }
00210         
00211         }
00212 
00213 }
00214 
00215 
00216 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00217 {
00218         (void)resultOut;
00219         (void)dispatchInfo;
00220         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
00221         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
00222 
00223 
00224         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
00225 
00226         //only perform CCD above a certain threshold, this prevents blocking on the long run
00227         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
00228         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
00229         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
00230         {
00231                 return btScalar(1.);
00232         }
00233 
00234         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
00235         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
00236         //todo: only do if the motion exceeds the 'radius'
00237 
00238         btTransform triInv = triBody->getWorldTransform().inverse();
00239         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
00240         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
00241 
00242         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
00243         {
00244                 btTransform m_ccdSphereFromTrans;
00245                 btTransform m_ccdSphereToTrans;
00246                 btTransform     m_meshTransform;
00247 
00248                 btScalar        m_ccdSphereRadius;
00249                 btScalar        m_hitFraction;
00250         
00251 
00252                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
00253                         :m_ccdSphereFromTrans(from),
00254                         m_ccdSphereToTrans(to),
00255                         m_ccdSphereRadius(ccdSphereRadius),
00256                         m_hitFraction(hitFraction)
00257                 {                       
00258                 }
00259                 
00260                 
00261                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
00262                 {
00263                         (void)partId;
00264                         (void)triangleIndex;
00265                         //do a swept sphere for now
00266                         btTransform ident;
00267                         ident.setIdentity();
00268                         btConvexCast::CastResult castResult;
00269                         castResult.m_fraction = m_hitFraction;
00270                         btSphereShape   pointShape(m_ccdSphereRadius);
00271                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
00272                         btVoronoiSimplexSolver  simplexSolver;
00273                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
00274                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
00275                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
00276                         //local space?
00277 
00278                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
00279                                 ident,ident,castResult))
00280                         {
00281                                 if (m_hitFraction > castResult.m_fraction)
00282                                         m_hitFraction = castResult.m_fraction;
00283                         }
00284 
00285                 }
00286 
00287         };
00288 
00289 
00290         
00291 
00292         
00293         if (triBody->getCollisionShape()->isConcave())
00294         {
00295                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
00296                 rayAabbMin.setMin(convexToLocal.getOrigin());
00297                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
00298                 rayAabbMax.setMax(convexToLocal.getOrigin());
00299                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
00300                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00301                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00302 
00303                 btScalar curHitFraction = btScalar(1.); //is this available?
00304                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
00305                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
00306 
00307                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
00308 
00309                 btCollisionObject* concavebody = triBody;
00310 
00311                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
00312                 
00313                 if (triangleMesh)
00314                 {
00315                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
00316                 }
00317         
00318 
00319 
00320                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
00321                 {
00322                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
00323                         return raycastCallback.m_hitFraction;
00324                 }
00325         }
00326 
00327         return btScalar(1.);
00328 
00329 }