Back to index

supertuxkart  0.5+dfsg1
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 
00029 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
00030 : btCollisionAlgorithm(ci),
00031 m_isSwapped(isSwapped),
00032 m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
00033 {
00034 }
00035 
00036 btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
00037 {
00038 }
00039 
00040 
00041 
00042 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
00043          m_dispatcher(dispatcher),
00044        m_dispatchInfoPtr(0)
00045 {
00046        m_convexBody = isSwapped? body1:body0;
00047        m_triBody = isSwapped? body0:body1;
00048        
00049          //
00050          // create the manifold from the dispatcher 'manifold pool'
00051          //
00052          m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
00053 
00054          clearCache();
00055 }
00056 
00057 btConvexTriangleCallback::~btConvexTriangleCallback()
00058 {
00059        clearCache();
00060        m_dispatcher->releaseManifold( m_manifoldPtr );
00061   
00062 }
00063   
00064 
00065 void   btConvexTriangleCallback::clearCache()
00066 {
00067        m_dispatcher->clearManifold(m_manifoldPtr);
00068 };
00069 
00070 
00071 
00072 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
00073 {
00074  
00075        //just for debugging purposes
00076        //printf("triangle %d",m_triangleCount++);
00077 
00078 
00079        //aabb filter is already applied!  
00080 
00081        btCollisionAlgorithmConstructionInfo ci;
00082        ci.m_dispatcher1 = m_dispatcher;
00083 
00084        btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
00085 
00086 
00087        
00089        if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
00090        {
00091               btVector3 color(255,255,0);
00092               btTransform& tr = ob->getWorldTransform();
00093               m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
00094               m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
00095               m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
00096 
00097               //btVector3 center = triangle[0] + triangle[1]+triangle[2];
00098               //center *= btScalar(0.333333);
00099               //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
00100               //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
00101               //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
00102 
00103        }
00104 
00105 
00106        //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
00107        
00108        if (m_convexBody->getCollisionShape()->isConvex())
00109        {
00110               btTriangleShape tm(triangle[0],triangle[1],triangle[2]);       
00111               tm.setMargin(m_collisionMarginTriangle);
00112               btCollisionShape* tmpShape = ob->getCollisionShape();
00113 
00114               //copy over user pointers to temporary shape
00115               tm.setUserPointer(tmpShape->getUserPointer());
00116               
00117               ob->setCollisionShape( &tm );
00118               
00119 
00120               btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
00122               //            btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
00123 
00124               m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
00125        //     cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
00126 //            cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
00127               colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
00128               colAlgo->~btCollisionAlgorithm();
00129               ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
00130               ob->setCollisionShape( tmpShape );
00131 
00132        }
00133 
00134        
00135 
00136 }
00137 
00138 
00139 
00140 void   btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00141 {
00142        m_dispatchInfoPtr = &dispatchInfo;
00143        m_collisionMarginTriangle = collisionMarginTriangle;
00144        m_resultOut = resultOut;
00145 
00146        //recalc aabbs
00147        btTransform convexInTriangleSpace;
00148        convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
00149        btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
00150        //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
00151        convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
00152        btScalar extraMargin = collisionMarginTriangle;
00153        btVector3 extra(extraMargin,extraMargin,extraMargin);
00154 
00155        m_aabbMax += extra;
00156        m_aabbMin -= extra;
00157        
00158 }
00159 
00160 void btConvexConcaveCollisionAlgorithm::clearCache()
00161 {
00162        m_btConvexTriangleCallback.clearCache();
00163 
00164 }
00165 
00166 void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00167 {
00168        
00169        
00170        btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
00171        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
00172 
00173        if (triBody->getCollisionShape()->isConcave())
00174        {
00175 
00176 
00177               btCollisionObject*   triOb = triBody;
00178               btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
00179               
00180               if (convexBody->getCollisionShape()->isConvex())
00181               {
00182                      btScalar collisionMarginTriangle = concaveShape->getMargin();
00183                                    
00184                      resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
00185                      m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
00186 
00187                      //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
00188                      //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
00189 
00190                      m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
00191 
00192                      concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
00193                      
00194                      resultOut->refreshContactPoints();
00195        
00196               }
00197        
00198        }
00199 
00200 }
00201 
00202 
00203 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00204 {
00205        (void)resultOut;
00206        (void)dispatchInfo;
00207        btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
00208        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
00209 
00210 
00211        //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
00212 
00213        //only perform CCD above a certain threshold, this prevents blocking on the long run
00214        //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
00215        btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
00216        if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
00217        {
00218               return btScalar(1.);
00219        }
00220 
00221        //const btVector3& from = convexbody->m_worldTransform.getOrigin();
00222        //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
00223        //todo: only do if the motion exceeds the 'radius'
00224 
00225        btTransform triInv = triBody->getWorldTransform().inverse();
00226        btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
00227        btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
00228 
00229        struct LocalTriangleSphereCastCallback    : public btTriangleCallback
00230        {
00231               btTransform m_ccdSphereFromTrans;
00232               btTransform m_ccdSphereToTrans;
00233               btTransform   m_meshTransform;
00234 
00235               btScalar      m_ccdSphereRadius;
00236               btScalar      m_hitFraction;
00237        
00238 
00239               LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
00240                      :m_ccdSphereFromTrans(from),
00241                      m_ccdSphereToTrans(to),
00242                      m_ccdSphereRadius(ccdSphereRadius),
00243                      m_hitFraction(hitFraction)
00244               {                    
00245               }
00246               
00247               
00248               virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
00249               {
00250                      (void)partId;
00251                      (void)triangleIndex;
00252                      //do a swept sphere for now
00253                      btTransform ident;
00254                      ident.setIdentity();
00255                      btConvexCast::CastResult castResult;
00256                      castResult.m_fraction = m_hitFraction;
00257                      btSphereShape pointShape(m_ccdSphereRadius);
00258                      btTriangleShape      triShape(triangle[0],triangle[1],triangle[2]);
00259                      btVoronoiSimplexSolver      simplexSolver;
00260                      btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
00261                      //GjkConvexCast      convexCaster(&pointShape,convexShape,&simplexSolver);
00262                      //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
00263                      //local space?
00264 
00265                      if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
00266                             ident,ident,castResult))
00267                      {
00268                             if (m_hitFraction > castResult.m_fraction)
00269                                    m_hitFraction = castResult.m_fraction;
00270                      }
00271 
00272               }
00273 
00274        };
00275 
00276 
00277        
00278 
00279        
00280        if (triBody->getCollisionShape()->isConcave())
00281        {
00282               btVector3 rayAabbMin = convexFromLocal.getOrigin();
00283               rayAabbMin.setMin(convexToLocal.getOrigin());
00284               btVector3 rayAabbMax = convexFromLocal.getOrigin();
00285               rayAabbMax.setMax(convexToLocal.getOrigin());
00286               btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
00287               rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00288               rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00289 
00290               btScalar curHitFraction = btScalar(1.); //is this available?
00291               LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
00292                      convexbody->getCcdSweptSphereRadius(),curHitFraction);
00293 
00294               raycastCallback.m_hitFraction = convexbody->getHitFraction();
00295 
00296               btCollisionObject* concavebody = triBody;
00297 
00298               btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
00299               
00300               if (triangleMesh)
00301               {
00302                      triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
00303               }
00304        
00305 
00306 
00307               if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
00308               {
00309                      convexbody->setHitFraction( raycastCallback.m_hitFraction);
00310                      return raycastCallback.m_hitFraction;
00311               }
00312        }
00313 
00314        return btScalar(1.);
00315 
00316 }