Blender V2.61 - r43446

btCollisionWorld.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 #include "btCollisionWorld.h"
00017 #include "btCollisionDispatcher.h"
00018 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00019 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
00020 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00022 #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
00023 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
00024 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
00025 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
00026 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00027 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
00028 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
00029 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
00030 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
00031 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
00032 #include "LinearMath/btAabbUtil2.h"
00033 #include "LinearMath/btQuickprof.h"
00034 #include "LinearMath/btStackAlloc.h"
00035 #include "LinearMath/btSerializer.h"
00036 
00037 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
00038 
00039 
00040 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
00041 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
00042 //#define RECALCULATE_AABB_RAYCAST 1
00043 
00044 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
00045 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00046 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
00047 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
00048 
00049 
00051 
00052 //for debug rendering
00053 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00054 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
00055 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
00056 #include "BulletCollision/CollisionShapes/btConeShape.h"
00057 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
00058 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
00059 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
00060 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
00061 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00062 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
00063 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
00064 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
00065 
00066 
00067 
00068 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
00069 :m_dispatcher1(dispatcher),
00070 m_broadphasePairCache(pairCache),
00071 m_debugDrawer(0),
00072 m_forceUpdateAllAabbs(true)
00073 {
00074     m_stackAlloc = collisionConfiguration->getStackAllocator();
00075     m_dispatchInfo.m_stackAllocator = m_stackAlloc;
00076 }
00077 
00078 
00079 btCollisionWorld::~btCollisionWorld()
00080 {
00081 
00082     //clean up remaining objects
00083     int i;
00084     for (i=0;i<m_collisionObjects.size();i++)
00085     {
00086         btCollisionObject* collisionObject= m_collisionObjects[i];
00087 
00088         btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
00089         if (bp)
00090         {
00091             //
00092             // only clear the cached algorithms
00093             //
00094             getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
00095             getBroadphase()->destroyProxy(bp,m_dispatcher1);
00096             collisionObject->setBroadphaseHandle(0);
00097         }
00098     }
00099 
00100 
00101 }
00102 
00103 
00104 
00105 
00106 
00107 
00108 
00109 
00110 
00111 
00112 void    btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
00113 {
00114 
00115     btAssert(collisionObject);
00116 
00117     //check that the object isn't already added
00118     btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
00119 
00120     m_collisionObjects.push_back(collisionObject);
00121 
00122     //calculate new AABB
00123     btTransform trans = collisionObject->getWorldTransform();
00124 
00125     btVector3   minAabb;
00126     btVector3   maxAabb;
00127     collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
00128 
00129     int type = collisionObject->getCollisionShape()->getShapeType();
00130     collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
00131         minAabb,
00132         maxAabb,
00133         type,
00134         collisionObject,
00135         collisionFilterGroup,
00136         collisionFilterMask,
00137         m_dispatcher1,0
00138         ))  ;
00139 
00140 
00141 
00142 
00143 
00144 }
00145 
00146 
00147 
00148 void    btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
00149 {
00150     btVector3 minAabb,maxAabb;
00151     colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
00152     //need to increase the aabb for contact thresholds
00153     btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
00154     minAabb -= contactThreshold;
00155     maxAabb += contactThreshold;
00156 
00157     if(getDispatchInfo().m_convexMaxDistanceUseCPT)
00158     {
00159         btVector3 minAabb2,maxAabb2;
00160         colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
00161         minAabb2 -= contactThreshold;
00162         maxAabb2 += contactThreshold;
00163         minAabb.setMin(minAabb2);
00164         maxAabb.setMax(maxAabb2);
00165     }
00166 
00167     btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
00168 
00169     //moving objects should be moderately sized, probably something wrong if not
00170     if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
00171     {
00172         bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
00173     } else
00174     {
00175         //something went wrong, investigate
00176         //this assert is unwanted in 3D modelers (danger of loosing work)
00177         colObj->setActivationState(DISABLE_SIMULATION);
00178 
00179         static bool reportMe = true;
00180         if (reportMe && m_debugDrawer)
00181         {
00182             reportMe = false;
00183             m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
00184             m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
00185             m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
00186             m_debugDrawer->reportErrorWarning("Thanks.\n");
00187         }
00188     }
00189 }
00190 
00191 void    btCollisionWorld::updateAabbs()
00192 {
00193     BT_PROFILE("updateAabbs");
00194 
00195     btTransform predictedTrans;
00196     for ( int i=0;i<m_collisionObjects.size();i++)
00197     {
00198         btCollisionObject* colObj = m_collisionObjects[i];
00199 
00200         //only update aabb of active objects
00201         if (m_forceUpdateAllAabbs || colObj->isActive())
00202         {
00203             updateSingleAabb(colObj);
00204         }
00205     }
00206 }
00207 
00208 
00209 
00210 void    btCollisionWorld::performDiscreteCollisionDetection()
00211 {
00212     BT_PROFILE("performDiscreteCollisionDetection");
00213 
00214     btDispatcherInfo& dispatchInfo = getDispatchInfo();
00215 
00216     updateAabbs();
00217 
00218     {
00219         BT_PROFILE("calculateOverlappingPairs");
00220         m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
00221     }
00222 
00223 
00224     btDispatcher* dispatcher = getDispatcher();
00225     {
00226         BT_PROFILE("dispatchAllCollisionPairs");
00227         if (dispatcher)
00228             dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
00229     }
00230 
00231 }
00232 
00233 
00234 
00235 void    btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
00236 {
00237 
00238 
00239     //bool removeFromBroadphase = false;
00240 
00241     {
00242 
00243         btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
00244         if (bp)
00245         {
00246             //
00247             // only clear the cached algorithms
00248             //
00249             getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
00250             getBroadphase()->destroyProxy(bp,m_dispatcher1);
00251             collisionObject->setBroadphaseHandle(0);
00252         }
00253     }
00254 
00255 
00256     //swapremove
00257     m_collisionObjects.remove(collisionObject);
00258 
00259 }
00260 
00261 
00262 
00263 void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
00264                                         btCollisionObject* collisionObject,
00265                                         const btCollisionShape* collisionShape,
00266                                         const btTransform& colObjWorldTransform,
00267                                         RayResultCallback& resultCallback)
00268 {
00269     btSphereShape pointShape(btScalar(0.0));
00270     pointShape.setMargin(0.f);
00271     const btConvexShape* castShape = &pointShape;
00272 
00273     if (collisionShape->isConvex())
00274     {
00275         //      BT_PROFILE("rayTestConvex");
00276         btConvexCast::CastResult castResult;
00277         castResult.m_fraction = resultCallback.m_closestHitFraction;
00278 
00279         btConvexShape* convexShape = (btConvexShape*) collisionShape;
00280         btVoronoiSimplexSolver  simplexSolver;
00281 #define USE_SUBSIMPLEX_CONVEX_CAST 1
00282 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
00283         btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
00284 #else
00285         //btGjkConvexCast   convexCaster(castShape,convexShape,&simplexSolver);
00286         //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
00287 #endif //#USE_SUBSIMPLEX_CONVEX_CAST
00288 
00289         if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
00290         {
00291             //add hit
00292             if (castResult.m_normal.length2() > btScalar(0.0001))
00293             {
00294                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
00295                 {
00296 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
00297                     //rotate normal into worldspace
00298                     castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
00299 #endif //USE_SUBSIMPLEX_CONVEX_CAST
00300 
00301                     castResult.m_normal.normalize();
00302                     btCollisionWorld::LocalRayResult localRayResult
00303                         (
00304                         collisionObject,
00305                         0,
00306                         castResult.m_normal,
00307                         castResult.m_fraction
00308                         );
00309 
00310                     bool normalInWorldSpace = true;
00311                     resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
00312 
00313                 }
00314             }
00315         }
00316     } else {
00317         if (collisionShape->isConcave())
00318         {
00319             //          BT_PROFILE("rayTestConcave");
00320             if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
00321             {
00323                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
00324                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
00325                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
00326                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
00327 
00328                 //ConvexCast::CastResult
00329                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
00330                 {
00331                     btCollisionWorld::RayResultCallback* m_resultCallback;
00332                     btCollisionObject*  m_collisionObject;
00333                     btTriangleMeshShape*    m_triangleMesh;
00334 
00335                     btTransform m_colObjWorldTransform;
00336 
00337                     BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
00338                         btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh,const btTransform& colObjWorldTransform):
00339                     //@BP Mod
00340                     btTriangleRaycastCallback(from,to, resultCallback->m_flags),
00341                         m_resultCallback(resultCallback),
00342                         m_collisionObject(collisionObject),
00343                         m_triangleMesh(triangleMesh),
00344                         m_colObjWorldTransform(colObjWorldTransform)
00345                     {
00346                     }
00347 
00348 
00349                     virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
00350                     {
00351                         btCollisionWorld::LocalShapeInfo    shapeInfo;
00352                         shapeInfo.m_shapePart = partId;
00353                         shapeInfo.m_triangleIndex = triangleIndex;
00354 
00355                         btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
00356 
00357                         btCollisionWorld::LocalRayResult rayResult
00358                             (m_collisionObject,
00359                             &shapeInfo,
00360                             hitNormalWorld,
00361                             hitFraction);
00362 
00363                         bool    normalInWorldSpace = true;
00364                         return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
00365                     }
00366 
00367                 };
00368 
00369                 BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
00370                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
00371                 triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
00372             } else
00373             {
00374                 //generic (slower) case
00375                 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
00376 
00377                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
00378 
00379                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
00380                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
00381 
00382                 //ConvexCast::CastResult
00383 
00384                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
00385                 {
00386                     btCollisionWorld::RayResultCallback* m_resultCallback;
00387                     btCollisionObject*  m_collisionObject;
00388                     btConcaveShape* m_triangleMesh;
00389 
00390                     btTransform m_colObjWorldTransform;
00391 
00392                     BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
00393                         btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
00394                     //@BP Mod
00395                     btTriangleRaycastCallback(from,to, resultCallback->m_flags),
00396                         m_resultCallback(resultCallback),
00397                         m_collisionObject(collisionObject),
00398                         m_triangleMesh(triangleMesh),
00399                         m_colObjWorldTransform(colObjWorldTransform)
00400                     {
00401                     }
00402 
00403 
00404                     virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
00405                     {
00406                         btCollisionWorld::LocalShapeInfo    shapeInfo;
00407                         shapeInfo.m_shapePart = partId;
00408                         shapeInfo.m_triangleIndex = triangleIndex;
00409 
00410                         btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
00411 
00412                         btCollisionWorld::LocalRayResult rayResult
00413                             (m_collisionObject,
00414                             &shapeInfo,
00415                             hitNormalWorld,
00416                             hitFraction);
00417 
00418                         bool    normalInWorldSpace = true;
00419                         return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
00420                     }
00421 
00422                 };
00423 
00424 
00425                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
00426                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
00427 
00428                 btVector3 rayAabbMinLocal = rayFromLocal;
00429                 rayAabbMinLocal.setMin(rayToLocal);
00430                 btVector3 rayAabbMaxLocal = rayFromLocal;
00431                 rayAabbMaxLocal.setMax(rayToLocal);
00432 
00433                 concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
00434             }
00435         } else {
00436             //          BT_PROFILE("rayTestCompound");
00437             if (collisionShape->isCompound())
00438             {
00439                 struct LocalInfoAdder2 : public RayResultCallback
00440                 {
00441                     RayResultCallback* m_userCallback;
00442                     int m_i;
00443                     
00444                     LocalInfoAdder2 (int i, RayResultCallback *user)
00445                         : m_userCallback(user), m_i(i)
00446                     { 
00447                         m_closestHitFraction = m_userCallback->m_closestHitFraction;
00448                     }
00449                     virtual bool needsCollision(btBroadphaseProxy* p) const
00450                     {
00451                         return m_userCallback->needsCollision(p);
00452                     }
00453 
00454                     virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
00455                     {
00456                         btCollisionWorld::LocalShapeInfo shapeInfo;
00457                         shapeInfo.m_shapePart = -1;
00458                         shapeInfo.m_triangleIndex = m_i;
00459                         if (r.m_localShapeInfo == NULL)
00460                             r.m_localShapeInfo = &shapeInfo;
00461 
00462                         const btScalar result = m_userCallback->addSingleResult(r, b);
00463                         m_closestHitFraction = m_userCallback->m_closestHitFraction;
00464                         return result;
00465                     }
00466                 };
00467                 
00468                 struct RayTester : btDbvt::ICollide
00469                 {
00470                     btCollisionObject* m_collisionObject;
00471                     const btCompoundShape* m_compoundShape;
00472                     const btTransform& m_colObjWorldTransform;
00473                     const btTransform& m_rayFromTrans;
00474                     const btTransform& m_rayToTrans;
00475                     RayResultCallback& m_resultCallback;
00476                     
00477                     RayTester(btCollisionObject* collisionObject,
00478                             const btCompoundShape* compoundShape,
00479                             const btTransform& colObjWorldTransform,
00480                             const btTransform& rayFromTrans,
00481                             const btTransform& rayToTrans,
00482                             RayResultCallback& resultCallback):
00483                         m_collisionObject(collisionObject),
00484                         m_compoundShape(compoundShape),
00485                         m_colObjWorldTransform(colObjWorldTransform),
00486                         m_rayFromTrans(rayFromTrans),
00487                         m_rayToTrans(rayToTrans),
00488                         m_resultCallback(resultCallback)
00489                     {
00490                         
00491                     }
00492                     
00493                     void Process(int i)
00494                     {
00495                         const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
00496                         const btTransform& childTrans = m_compoundShape->getChildTransform(i);
00497                         btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
00498                         
00499                         // replace collision shape so that callback can determine the triangle
00500                         btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape();
00501                         m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
00502 
00503                         LocalInfoAdder2 my_cb(i, &m_resultCallback);
00504 
00505                         rayTestSingle(
00506                             m_rayFromTrans,
00507                             m_rayToTrans,
00508                             m_collisionObject,
00509                             childCollisionShape,
00510                             childWorldTrans,
00511                             my_cb);
00512                         
00513                         // restore
00514                         m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
00515                     }
00516                     
00517                     void Process(const btDbvtNode* leaf)
00518                     {
00519                         Process(leaf->dataAsInt);
00520                     }
00521                 };
00522                 
00523                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
00524                 const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
00525 
00526 
00527                 RayTester rayCB(
00528                     collisionObject,
00529                     compoundShape,
00530                     colObjWorldTransform,
00531                     rayFromTrans,
00532                     rayToTrans,
00533                     resultCallback);
00534 #ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
00535                 if (dbvt)
00536                 {
00537                     btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
00538                     btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
00539                     btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
00540                 }
00541                 else
00542 #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
00543                 {
00544                     for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
00545                     {
00546                         rayCB.Process(i);
00547                     }   
00548                 }
00549             }
00550         }
00551     }
00552 }
00553 
00554 void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
00555                                             btCollisionObject* collisionObject,
00556                                             const btCollisionShape* collisionShape,
00557                                             const btTransform& colObjWorldTransform,
00558                                             ConvexResultCallback& resultCallback, btScalar allowedPenetration)
00559 {
00560     if (collisionShape->isConvex())
00561     {
00562         //BT_PROFILE("convexSweepConvex");
00563         btConvexCast::CastResult castResult;
00564         castResult.m_allowedPenetration = allowedPenetration;
00565         castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
00566 
00567         btConvexShape* convexShape = (btConvexShape*) collisionShape;
00568         btVoronoiSimplexSolver  simplexSolver;
00569         btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
00570 
00571         btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
00572         //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
00573         //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
00574 
00575         btConvexCast* castPtr = &convexCaster1;
00576 
00577 
00578 
00579         if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
00580         {
00581             //add hit
00582             if (castResult.m_normal.length2() > btScalar(0.0001))
00583             {
00584                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
00585                 {
00586                     castResult.m_normal.normalize();
00587                     btCollisionWorld::LocalConvexResult localConvexResult
00588                         (
00589                         collisionObject,
00590                         0,
00591                         castResult.m_normal,
00592                         castResult.m_hitPoint,
00593                         castResult.m_fraction
00594                         );
00595 
00596                     bool normalInWorldSpace = true;
00597                     resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
00598 
00599                 }
00600             }
00601         }
00602     } else {
00603         if (collisionShape->isConcave())
00604         {
00605             if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
00606             {
00607                 //BT_PROFILE("convexSweepbtBvhTriangleMesh");
00608                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
00609                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
00610                 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
00611                 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
00612                 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
00613                 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
00614 
00615                 //ConvexCast::CastResult
00616                 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
00617                 {
00618                     btCollisionWorld::ConvexResultCallback* m_resultCallback;
00619                     btCollisionObject*  m_collisionObject;
00620                     btTriangleMeshShape*    m_triangleMesh;
00621 
00622                     BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
00623                         btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
00624                     btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
00625                         m_resultCallback(resultCallback),
00626                         m_collisionObject(collisionObject),
00627                         m_triangleMesh(triangleMesh)
00628                     {
00629                     }
00630 
00631 
00632                     virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
00633                     {
00634                         btCollisionWorld::LocalShapeInfo    shapeInfo;
00635                         shapeInfo.m_shapePart = partId;
00636                         shapeInfo.m_triangleIndex = triangleIndex;
00637                         if (hitFraction <= m_resultCallback->m_closestHitFraction)
00638                         {
00639 
00640                             btCollisionWorld::LocalConvexResult convexResult
00641                                 (m_collisionObject,
00642                                 &shapeInfo,
00643                                 hitNormalLocal,
00644                                 hitPointLocal,
00645                                 hitFraction);
00646 
00647                             bool    normalInWorldSpace = true;
00648 
00649 
00650                             return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
00651                         }
00652                         return hitFraction;
00653                     }
00654 
00655                 };
00656 
00657                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
00658                 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
00659                 tccb.m_allowedPenetration = allowedPenetration;
00660                 btVector3 boxMinLocal, boxMaxLocal;
00661                 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
00662                 triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
00663             } else
00664             {
00665                 //BT_PROFILE("convexSweepConcave");
00666                 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
00667                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
00668                 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
00669                 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
00670                 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
00671                 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
00672 
00673                 //ConvexCast::CastResult
00674                 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
00675                 {
00676                     btCollisionWorld::ConvexResultCallback* m_resultCallback;
00677                     btCollisionObject*  m_collisionObject;
00678                     btConcaveShape* m_triangleMesh;
00679 
00680                     BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
00681                         btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*  triangleMesh, const btTransform& triangleToWorld):
00682                     btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
00683                         m_resultCallback(resultCallback),
00684                         m_collisionObject(collisionObject),
00685                         m_triangleMesh(triangleMesh)
00686                     {
00687                     }
00688 
00689 
00690                     virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
00691                     {
00692                         btCollisionWorld::LocalShapeInfo    shapeInfo;
00693                         shapeInfo.m_shapePart = partId;
00694                         shapeInfo.m_triangleIndex = triangleIndex;
00695                         if (hitFraction <= m_resultCallback->m_closestHitFraction)
00696                         {
00697 
00698                             btCollisionWorld::LocalConvexResult convexResult
00699                                 (m_collisionObject,
00700                                 &shapeInfo,
00701                                 hitNormalLocal,
00702                                 hitPointLocal,
00703                                 hitFraction);
00704 
00705                             bool    normalInWorldSpace = false;
00706 
00707                             return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
00708                         }
00709                         return hitFraction;
00710                     }
00711 
00712                 };
00713 
00714                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
00715                 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
00716                 tccb.m_allowedPenetration = allowedPenetration;
00717                 btVector3 boxMinLocal, boxMaxLocal;
00718                 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
00719 
00720                 btVector3 rayAabbMinLocal = convexFromLocal;
00721                 rayAabbMinLocal.setMin(convexToLocal);
00722                 btVector3 rayAabbMaxLocal = convexFromLocal;
00723                 rayAabbMaxLocal.setMax(convexToLocal);
00724                 rayAabbMinLocal += boxMinLocal;
00725                 rayAabbMaxLocal += boxMaxLocal;
00726                 concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
00727             }
00728         } else {
00730             if (collisionShape->isCompound())
00731             {
00732                 BT_PROFILE("convexSweepCompound");
00733                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
00734                 int i=0;
00735                 for (i=0;i<compoundShape->getNumChildShapes();i++)
00736                 {
00737                     btTransform childTrans = compoundShape->getChildTransform(i);
00738                     const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
00739                     btTransform childWorldTrans = colObjWorldTransform * childTrans;
00740                     // replace collision shape so that callback can determine the triangle
00741                     btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
00742                     collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
00743                     struct  LocalInfoAdder : public ConvexResultCallback {
00744                             ConvexResultCallback* m_userCallback;
00745                             int m_i;
00746 
00747                             LocalInfoAdder (int i, ConvexResultCallback *user)
00748                                 : m_userCallback(user), m_i(i)
00749                             {
00750                                 m_closestHitFraction = m_userCallback->m_closestHitFraction;
00751                             }
00752                             virtual bool needsCollision(btBroadphaseProxy* p) const
00753                             {
00754                                 return m_userCallback->needsCollision(p);
00755                             }
00756                             virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&  r,  bool b)
00757                             {
00758                                     btCollisionWorld::LocalShapeInfo    shapeInfo;
00759                                     shapeInfo.m_shapePart = -1;
00760                                     shapeInfo.m_triangleIndex = m_i;
00761                                     if (r.m_localShapeInfo == NULL)
00762                                         r.m_localShapeInfo = &shapeInfo;
00763                                     const btScalar result = m_userCallback->addSingleResult(r, b);
00764                                     m_closestHitFraction = m_userCallback->m_closestHitFraction;
00765                                     return result;
00766                                     
00767                             }
00768                     };
00769 
00770                     LocalInfoAdder my_cb(i, &resultCallback);
00771                     
00772 
00773                     objectQuerySingle(castShape, convexFromTrans,convexToTrans,
00774                         collisionObject,
00775                         childCollisionShape,
00776                         childWorldTrans,
00777                         my_cb, allowedPenetration);
00778                     // restore
00779                     collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
00780                 }
00781             }
00782         }
00783     }
00784 }
00785 
00786 
00787 struct btSingleRayCallback : public btBroadphaseRayCallback
00788 {
00789 
00790     btVector3   m_rayFromWorld;
00791     btVector3   m_rayToWorld;
00792     btTransform m_rayFromTrans;
00793     btTransform m_rayToTrans;
00794     btVector3   m_hitNormal;
00795 
00796     const btCollisionWorld* m_world;
00797     btCollisionWorld::RayResultCallback&    m_resultCallback;
00798 
00799     btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
00800         :m_rayFromWorld(rayFromWorld),
00801         m_rayToWorld(rayToWorld),
00802         m_world(world),
00803         m_resultCallback(resultCallback)
00804     {
00805         m_rayFromTrans.setIdentity();
00806         m_rayFromTrans.setOrigin(m_rayFromWorld);
00807         m_rayToTrans.setIdentity();
00808         m_rayToTrans.setOrigin(m_rayToWorld);
00809 
00810         btVector3 rayDir = (rayToWorld-rayFromWorld);
00811 
00812         rayDir.normalize ();
00814         m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
00815         m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
00816         m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
00817         m_signs[0] = m_rayDirectionInverse[0] < 0.0;
00818         m_signs[1] = m_rayDirectionInverse[1] < 0.0;
00819         m_signs[2] = m_rayDirectionInverse[2] < 0.0;
00820 
00821         m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
00822 
00823     }
00824 
00825 
00826 
00827     virtual bool    process(const btBroadphaseProxy* proxy)
00828     {
00830         if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
00831             return false;
00832 
00833         btCollisionObject*  collisionObject = (btCollisionObject*)proxy->m_clientObject;
00834 
00835         //only perform raycast if filterMask matches
00836         if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
00837         {
00838             //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
00839             //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
00840 #if 0
00841 #ifdef RECALCULATE_AABB
00842             btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
00843             collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
00844 #else
00845             //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
00846             const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
00847             const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
00848 #endif
00849 #endif
00850             //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
00851             //culling already done by broadphase
00852             //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
00853             {
00854                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
00855                     collisionObject,
00856                     collisionObject->getCollisionShape(),
00857                     collisionObject->getWorldTransform(),
00858                     m_resultCallback);
00859             }
00860         }
00861         return true;
00862     }
00863 };
00864 
00865 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
00866 {
00867     //BT_PROFILE("rayTest");
00870     btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
00871 
00872 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
00873     m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
00874 #else
00875     for (int i=0;i<this->getNumCollisionObjects();i++)
00876     {
00877         rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
00878     }   
00879 #endif //USE_BRUTEFORCE_RAYBROADPHASE
00880 
00881 }
00882 
00883 
00884 struct btSingleSweepCallback : public btBroadphaseRayCallback
00885 {
00886 
00887     btTransform m_convexFromTrans;
00888     btTransform m_convexToTrans;
00889     btVector3   m_hitNormal;
00890     const btCollisionWorld* m_world;
00891     btCollisionWorld::ConvexResultCallback& m_resultCallback;
00892     btScalar    m_allowedCcdPenetration;
00893     const btConvexShape* m_castShape;
00894 
00895 
00896     btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
00897         :m_convexFromTrans(convexFromTrans),
00898         m_convexToTrans(convexToTrans),
00899         m_world(world),
00900         m_resultCallback(resultCallback),
00901         m_allowedCcdPenetration(allowedPenetration),
00902         m_castShape(castShape)
00903     {
00904         btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
00905         btVector3 rayDir = unnormalizedRayDir.normalized();
00907         m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
00908         m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
00909         m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
00910         m_signs[0] = m_rayDirectionInverse[0] < 0.0;
00911         m_signs[1] = m_rayDirectionInverse[1] < 0.0;
00912         m_signs[2] = m_rayDirectionInverse[2] < 0.0;
00913 
00914         m_lambda_max = rayDir.dot(unnormalizedRayDir);
00915 
00916     }
00917 
00918     virtual bool    process(const btBroadphaseProxy* proxy)
00919     {
00921         if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
00922             return false;
00923 
00924         btCollisionObject*  collisionObject = (btCollisionObject*)proxy->m_clientObject;
00925 
00926         //only perform raycast if filterMask matches
00927         if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
00928             //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
00929             m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
00930                 collisionObject,
00931                 collisionObject->getCollisionShape(),
00932                 collisionObject->getWorldTransform(),
00933                 m_resultCallback,
00934                 m_allowedCcdPenetration);
00935         }
00936 
00937         return true;
00938     }
00939 };
00940 
00941 
00942 
00943 void    btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
00944 {
00945 
00946     BT_PROFILE("convexSweepTest");
00950 
00951 
00952 
00953     btTransform convexFromTrans,convexToTrans;
00954     convexFromTrans = convexFromWorld;
00955     convexToTrans = convexToWorld;
00956     btVector3 castShapeAabbMin, castShapeAabbMax;
00957     /* Compute AABB that encompasses angular movement */
00958     {
00959         btVector3 linVel, angVel;
00960         btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
00961         btVector3 zeroLinVel;
00962         zeroLinVel.setValue(0,0,0);
00963         btTransform R;
00964         R.setIdentity ();
00965         R.setRotation (convexFromTrans.getRotation());
00966         castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
00967     }
00968 
00969 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
00970 
00971     btSingleSweepCallback   convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
00972 
00973     m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
00974 
00975 #else
00976 
00977     // do a ray-shape query using convexCaster (CCD)
00978     int i;
00979     for (i=0;i<m_collisionObjects.size();i++)
00980     {
00981         btCollisionObject*  collisionObject= m_collisionObjects[i];
00982         //only perform raycast if filterMask matches
00983         if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
00984             //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
00985             btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
00986             collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
00987             AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
00988             btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
00989             btVector3 hitNormal;
00990             if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
00991             {
00992                 objectQuerySingle(castShape, convexFromTrans,convexToTrans,
00993                     collisionObject,
00994                     collisionObject->getCollisionShape(),
00995                     collisionObject->getWorldTransform(),
00996                     resultCallback,
00997                     allowedCcdPenetration);
00998             }
00999         }
01000     }
01001 #endif //USE_BRUTEFORCE_RAYBROADPHASE
01002 }
01003 
01004 
01005 
01006 struct btBridgedManifoldResult : public btManifoldResult
01007 {
01008 
01009     btCollisionWorld::ContactResultCallback&    m_resultCallback;
01010 
01011     btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
01012         :btManifoldResult(obj0,obj1),
01013         m_resultCallback(resultCallback)
01014     {
01015     }
01016 
01017     virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
01018     {
01019         bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
01020         btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
01021         btVector3 localA;
01022         btVector3 localB;
01023         if (isSwapped)
01024         {
01025             localA = m_rootTransB.invXform(pointA );
01026             localB = m_rootTransA.invXform(pointInWorld);
01027         } else
01028         {
01029             localA = m_rootTransA.invXform(pointA );
01030             localB = m_rootTransB.invXform(pointInWorld);
01031         }
01032         
01033         btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
01034         newPt.m_positionWorldOnA = pointA;
01035         newPt.m_positionWorldOnB = pointInWorld;
01036         
01037        //BP mod, store contact triangles.
01038         if (isSwapped)
01039         {
01040             newPt.m_partId0 = m_partId1;
01041             newPt.m_partId1 = m_partId0;
01042             newPt.m_index0  = m_index1;
01043             newPt.m_index1  = m_index0;
01044         } else
01045         {
01046             newPt.m_partId0 = m_partId0;
01047             newPt.m_partId1 = m_partId1;
01048             newPt.m_index0  = m_index0;
01049             newPt.m_index1  = m_index1;
01050         }
01051 
01052         //experimental feature info, for per-triangle material etc.
01053         btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
01054         btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
01055         m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
01056 
01057     }
01058     
01059 };
01060 
01061 
01062 
01063 struct btSingleContactCallback : public btBroadphaseAabbCallback
01064 {
01065 
01066     btCollisionObject* m_collisionObject;
01067     btCollisionWorld*   m_world;
01068     btCollisionWorld::ContactResultCallback&    m_resultCallback;
01069     
01070     
01071     btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
01072         :m_collisionObject(collisionObject),
01073         m_world(world),
01074         m_resultCallback(resultCallback)
01075     {
01076     }
01077 
01078     virtual bool    process(const btBroadphaseProxy* proxy)
01079     {
01080         btCollisionObject*  collisionObject = (btCollisionObject*)proxy->m_clientObject;
01081         if (collisionObject == m_collisionObject)
01082             return true;
01083 
01084         //only perform raycast if filterMask matches
01085         if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
01086         {
01087             btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
01088             if (algorithm)
01089             {
01090                 btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
01091                 //discrete collision detection query
01092                 algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
01093 
01094                 algorithm->~btCollisionAlgorithm();
01095                 m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
01096             }
01097         }
01098         return true;
01099     }
01100 };
01101 
01102 
01105 void    btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
01106 {
01107     btVector3 aabbMin,aabbMax;
01108     colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
01109     btSingleContactCallback contactCB(colObj,this,resultCallback);
01110     
01111     m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
01112 }
01113 
01114 
01117 void    btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
01118 {
01119     btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
01120     if (algorithm)
01121     {
01122         btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
01123         //discrete collision detection query
01124         algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
01125 
01126         algorithm->~btCollisionAlgorithm();
01127         getDispatcher()->freeCollisionAlgorithm(algorithm);
01128     }
01129 
01130 }
01131 
01132 
01133 
01134 
01135 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
01136 {
01137     btIDebugDraw*   m_debugDrawer;
01138     btVector3   m_color;
01139     btTransform m_worldTrans;
01140 
01141 public:
01142 
01143     DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
01144       m_debugDrawer(debugDrawer),
01145           m_color(color),
01146           m_worldTrans(worldTrans)
01147       {
01148       }
01149 
01150       virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
01151       {
01152           processTriangle(triangle,partId,triangleIndex);
01153       }
01154 
01155       virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
01156       {
01157           (void)partId;
01158           (void)triangleIndex;
01159 
01160           btVector3 wv0,wv1,wv2;
01161           wv0 = m_worldTrans*triangle[0];
01162           wv1 = m_worldTrans*triangle[1];
01163           wv2 = m_worldTrans*triangle[2];
01164           btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
01165 
01166           btVector3 normal = (wv1-wv0).cross(wv2-wv0);
01167           normal.normalize();
01168           btVector3 normalColor(1,1,0);
01169           m_debugDrawer->drawLine(center,center+normal,normalColor);
01170 
01171 
01172 
01173          
01174           m_debugDrawer->drawLine(wv0,wv1,m_color);
01175           m_debugDrawer->drawLine(wv1,wv2,m_color);
01176           m_debugDrawer->drawLine(wv2,wv0,m_color);
01177       }
01178 };
01179 
01180 
01181 void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
01182 {
01183     // Draw a small simplex at the center of the object
01184     getDebugDrawer()->drawTransform(worldTransform,1);
01185 
01186     if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
01187     {
01188         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
01189         for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
01190         {
01191             btTransform childTrans = compoundShape->getChildTransform(i);
01192             const btCollisionShape* colShape = compoundShape->getChildShape(i);
01193             debugDrawObject(worldTransform*childTrans,colShape,color);
01194         }
01195 
01196     } else
01197     {
01198         switch (shape->getShapeType())
01199         {
01200 
01201         case BOX_SHAPE_PROXYTYPE:
01202             {
01203                 const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
01204                 btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
01205                 getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
01206                 break;
01207             }
01208 
01209         case SPHERE_SHAPE_PROXYTYPE:
01210             {
01211                 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
01212                 btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
01213 
01214                 getDebugDrawer()->drawSphere(radius, worldTransform, color);
01215                 break;
01216             }
01217         case MULTI_SPHERE_SHAPE_PROXYTYPE:
01218             {
01219                 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
01220 
01221                 btTransform childTransform;
01222                 childTransform.setIdentity();
01223 
01224                 for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
01225                 {
01226                     childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
01227                     getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
01228                 }
01229 
01230                 break;
01231             }
01232         case CAPSULE_SHAPE_PROXYTYPE:
01233             {
01234                 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
01235 
01236                 btScalar radius = capsuleShape->getRadius();
01237                 btScalar halfHeight = capsuleShape->getHalfHeight();
01238 
01239                 int upAxis = capsuleShape->getUpAxis();
01240                 getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
01241                 break;
01242             }
01243         case CONE_SHAPE_PROXYTYPE:
01244             {
01245                 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
01246                 btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
01247                 btScalar height = coneShape->getHeight();//+coneShape->getMargin();
01248 
01249                 int upAxis= coneShape->getConeUpIndex();
01250                 getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
01251                 break;
01252 
01253             }
01254         case CYLINDER_SHAPE_PROXYTYPE:
01255             {
01256                 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
01257                 int upAxis = cylinder->getUpAxis();
01258                 btScalar radius = cylinder->getRadius();
01259                 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
01260                 getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
01261                 break;
01262             }
01263 
01264         case STATIC_PLANE_PROXYTYPE:
01265             {
01266                 const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
01267                 btScalar planeConst = staticPlaneShape->getPlaneConstant();
01268                 const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
01269                 getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
01270                 break;
01271 
01272             }
01273         default:
01274             {
01275 
01276                 if (shape->isConcave())
01277                 {
01278                     btConcaveShape* concaveMesh = (btConcaveShape*) shape;
01279 
01281                     btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
01282                     btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
01283 
01284                     DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
01285                     concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
01286 
01287                 }
01288 
01289                 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
01290                 {
01291                     btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
01292                     //todo: pass camera for some culling            
01293                     btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
01294                     btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
01295                     //DebugDrawcallback drawCallback;
01296                     DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
01297                     convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
01298                 }
01299 
01300 
01302                 if (shape->isPolyhedral())
01303                 {
01304                     btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
01305 
01306                     int i;
01307                     for (i=0;i<polyshape->getNumEdges();i++)
01308                     {
01309                         btVector3 a,b;
01310                         polyshape->getEdge(i,a,b);
01311                         btVector3 wa = worldTransform * a;
01312                         btVector3 wb = worldTransform * b;
01313                         getDebugDrawer()->drawLine(wa,wb,color);
01314 
01315                     }
01316 
01317 
01318                 }
01319             }
01320         }
01321     }
01322 }
01323 
01324 
01325 void    btCollisionWorld::debugDrawWorld()
01326 {
01327     if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
01328     {
01329         int numManifolds = getDispatcher()->getNumManifolds();
01330         btVector3 color(0,0,0);
01331         for (int i=0;i<numManifolds;i++)
01332         {
01333             btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
01334             //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
01335             //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
01336 
01337             int numContacts = contactManifold->getNumContacts();
01338             for (int j=0;j<numContacts;j++)
01339             {
01340                 btManifoldPoint& cp = contactManifold->getContactPoint(j);
01341                 getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
01342             }
01343         }
01344     }
01345 
01346     if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
01347     {
01348         int i;
01349 
01350         for (  i=0;i<m_collisionObjects.size();i++)
01351         {
01352             btCollisionObject* colObj = m_collisionObjects[i];
01353             if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
01354             {
01355                 if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
01356                 {
01357                     btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
01358                     switch(colObj->getActivationState())
01359                     {
01360                     case  ACTIVE_TAG:
01361                         color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
01362                     case ISLAND_SLEEPING:
01363                         color =  btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
01364                     case WANTS_DEACTIVATION:
01365                         color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
01366                     case DISABLE_DEACTIVATION:
01367                         color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
01368                     case DISABLE_SIMULATION:
01369                         color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
01370                     default:
01371                         {
01372                             color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
01373                         }
01374                     };
01375 
01376                     debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
01377                 }
01378                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
01379                 {
01380                     btVector3 minAabb,maxAabb;
01381                     btVector3 colorvec(1,0,0);
01382                     colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
01383                     btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
01384                     minAabb -= contactThreshold;
01385                     maxAabb += contactThreshold;
01386 
01387                     btVector3 minAabb2,maxAabb2;
01388 
01389                     colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
01390                     minAabb2 -= contactThreshold;
01391                     maxAabb2 += contactThreshold;
01392 
01393                     minAabb.setMin(minAabb2);
01394                     maxAabb.setMax(maxAabb2);
01395 
01396                     m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
01397                 }
01398             }
01399 
01400         }
01401     }
01402 }
01403 
01404 
01405 void    btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
01406 {
01407     int i;
01408     //serialize all collision objects
01409     for (i=0;i<m_collisionObjects.size();i++)
01410     {
01411         btCollisionObject* colObj = m_collisionObjects[i];
01412         if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
01413         {
01414             colObj->serializeSingleObject(serializer);
01415         }
01416     }
01417 
01419     btHashMap<btHashPtr,btCollisionShape*>  serializedShapes;
01420 
01421     for (i=0;i<m_collisionObjects.size();i++)
01422     {
01423         btCollisionObject* colObj = m_collisionObjects[i];
01424         btCollisionShape* shape = colObj->getCollisionShape();
01425 
01426         if (!serializedShapes.find(shape))
01427         {
01428             serializedShapes.insert(shape,shape);
01429             shape->serializeSingleShape(serializer);
01430         }
01431     }
01432 
01433 }
01434 
01435 
01436 void    btCollisionWorld::serialize(btSerializer* serializer)
01437 {
01438 
01439     serializer->startSerialization();
01440     
01441     serializeCollisionObjects(serializer);
01442     
01443     serializer->finishSerialization();
01444 }
01445