src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btCollisionWorld.h"
  17 #include "btCollisionDispatcher.h"
  18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  19 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
  20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  21 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
  22 #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
  23 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
  24 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
  25 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
  26 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  27 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
  28 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
  29 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
  30 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
  31 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
  32 #include "LinearMath/btAabbUtil2.h"
  33 #include "LinearMath/btQuickprof.h"
  34 #include "LinearMath/btStackAlloc.h"
  35 #include "LinearMath/btSerializer.h"
  36 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
  37 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  38
  39 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
  40
  41
  42 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
  43 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
  44 //#define RECALCULATE_AABB_RAYCAST 1
  45
  46 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
  47 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  48 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
  49 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
  50
  51
  52 ///for debug drawing
  53
  54 //for debug rendering
  55 #include "BulletCollision/CollisionShapes/btBoxShape.h"
  56 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
  57 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
  58 #include "BulletCollision/CollisionShapes/btConeShape.h"
  59 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
  60 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
  61 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
  62 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
  63 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  64 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
  65 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
  66 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
  67
  68
  69
  70 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
  71 :m_dispatcher1(dispatcher),
  72 m_broadphasePairCache(pairCache),
  73 m_debugDrawer(0),
  74 m_forceUpdateAllAabbs(true)
  75 {
  76         m_stackAlloc = collisionConfiguration->getStackAllocator();
  77         m_dispatchInfo.m_stackAllocator = m_stackAlloc;
  78 }
  79
  80
  81 btCollisionWorld::~btCollisionWorld()
  82 {
  83
  84         //clean up remaining objects
  85         int i;
  86         for (i=0;i<m_collisionObjects.size();i++)
  87         {
  88                 btCollisionObject* collisionObject= m_collisionObjects[i];
  89
  90                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
  91                 if (bp)
  92                 {
  93                         //
  94                         // only clear the cached algorithms
  95                         //
  96                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
  97                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
  98                         collisionObject->setBroadphaseHandle(0);
  99                 }
 100         }
 101
 102
 103 }
 104
 105
 106
 107
 108
 109
 110
 111
 112
 113
 114 void    btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
 115 {
 116
 117         btAssert(collisionObject);
 118
 119         //check that the object isn't already added
 120         btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
 121
 122         m_collisionObjects.push_back(collisionObject);
 123
 124         //calculate new AABB
 125         btTransform trans = collisionObject->getWorldTransform();
 126
 127         btVector3       minAabb;
 128         btVector3       maxAabb;
 129         collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
 130
 131         int type = collisionObject->getCollisionShape()->getShapeType();
 132         collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
 133                 minAabb,
 134                 maxAabb,
 135                 type,
 136                 collisionObject,
 137                 collisionFilterGroup,
 138                 collisionFilterMask,
 139                 m_dispatcher1,0
 140                 ))      ;
 141
 142
 143
 144
 145
 146 }
 147
 148
 149
 150 void    btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
 151 {
 152         btVector3 minAabb,maxAabb;
 153         colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
 154         //need to increase the aabb for contact thresholds
 155         btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
 156         minAabb -= contactThreshold;
 157         maxAabb += contactThreshold;
 158
 159         if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
 160         {
 161                 btVector3 minAabb2,maxAabb2;
 162                 colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
 163                 minAabb2 -= contactThreshold;
 164                 maxAabb2 += contactThreshold;
 165                 minAabb.setMin(minAabb2);
 166                 maxAabb.setMax(maxAabb2);
 167         }
 168
 169         btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
 170
 171         //moving objects should be moderately sized, probably something wrong if not
 172         if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
 173         {
 174                 bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 175         } else
 176         {
 177                 //something went wrong, investigate
 178                 //this assert is unwanted in 3D modelers (danger of loosing work)
 179                 colObj->setActivationState(DISABLE_SIMULATION);
 180
 181                 static bool reportMe = true;
 182                 if (reportMe && m_debugDrawer)
 183                 {
 184                         reportMe = false;
 185                         m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
 186                         m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
 187                         m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
 188                         m_debugDrawer->reportErrorWarning("Thanks.\n");
 189                 }
 190         }
 191 }
 192
 193 void    btCollisionWorld::updateAabbs()
 194 {
 195         BT_PROFILE("updateAabbs");
 196
 197         btTransform predictedTrans;
 198         for ( int i=0;i<m_collisionObjects.size();i++)
 199         {
 200                 btCollisionObject* colObj = m_collisionObjects[i];
 201
 202                 //only update aabb of active objects
 203                 if (m_forceUpdateAllAabbs || colObj->isActive())
 204                 {
 205                         updateSingleAabb(colObj);
 206                 }
 207         }
 208 }
 209
 210
 211 void    btCollisionWorld::computeOverlappingPairs()
 212 {
 213         BT_PROFILE("calculateOverlappingPairs");
 214         m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
 215 }
 216
 217 void    btCollisionWorld::performDiscreteCollisionDetection()
 218 {
 219         BT_PROFILE("performDiscreteCollisionDetection");
 220
 221         btDispatcherInfo& dispatchInfo = getDispatchInfo();
 222
 223         updateAabbs();
 224
 225         computeOverlappingPairs();
 226
 227         btDispatcher* dispatcher = getDispatcher();
 228         {
 229                 BT_PROFILE("dispatchAllCollisionPairs");
 230                 if (dispatcher)
 231                         dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
 232         }
 233
 234 }
 235
 236
 237
 238 void    btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 239 {
 240
 241
 242         //bool removeFromBroadphase = false;
 243
 244         {
 245
 246                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
 247                 if (bp)
 248                 {
 249                         //
 250                         // only clear the cached algorithms
 251                         //
 252                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
 253                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
 254                         collisionObject->setBroadphaseHandle(0);
 255                 }
 256         }
 257
 258
 259         //swapremove
 260         m_collisionObjects.remove(collisionObject);
 261
 262 }
 263
 264
 265 void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
 266                                                                                 btCollisionObject* collisionObject,
 267                                                                                 const btCollisionShape* collisionShape,
 268                                                                                 const btTransform& colObjWorldTransform,
 269                                                                                 RayResultCallback& resultCallback)
 270 {
 271         btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform);
 272         btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback);
 273 }
 274
 275 void    btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans,
 276                                                                                 const btCollisionObjectWrapper* collisionObjectWrap,
 277                                                                                 RayResultCallback& resultCallback)
 278 {
 279         btSphereShape pointShape(btScalar(0.0));
 280         pointShape.setMargin(0.f);
 281         const btConvexShape* castShape = &pointShape;
 282         const btCollisionShape* collisionShape = collisionObjectWrap->getCollisionShape();
 283         const btTransform& colObjWorldTransform = collisionObjectWrap->getWorldTransform();
 284
 285         if (collisionShape->isConvex())
 286         {
 287                 //              BT_PROFILE("rayTestConvex");
 288                 btConvexCast::CastResult castResult;
 289                 castResult.m_fraction = resultCallback.m_closestHitFraction;
 290
 291                 btConvexShape* convexShape = (btConvexShape*) collisionShape;
 292                 btVoronoiSimplexSolver  simplexSolver;
 293 #define USE_SUBSIMPLEX_CONVEX_CAST 1
 294 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
 295                 btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
 296 #else
 297                 //btGjkConvexCast       convexCaster(castShape,convexShape,&simplexSolver);
 298                 //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
 299 #endif //#USE_SUBSIMPLEX_CONVEX_CAST
 300
 301                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
 302                 {
 303                         //add hit
 304                         if (castResult.m_normal.length2() > btScalar(0.0001))
 305                         {
 306                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 307                                 {
 308 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
 309                                         //rotate normal into worldspace
 310                                         castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
 311 #endif //USE_SUBSIMPLEX_CONVEX_CAST
 312
 313                                         castResult.m_normal.normalize();
 314                                         btCollisionWorld::LocalRayResult localRayResult
 315                                                 (
 316                                                 collisionObjectWrap->getCollisionObject(),
 317                                                 0,
 318                                                 castResult.m_normal,
 319                                                 castResult.m_fraction
 320                                                 );
 321
 322                                         bool normalInWorldSpace = true;
 323                                         resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
 324
 325                                 }
 326                         }
 327                 }
 328         } else {
 329                 if (collisionShape->isConcave())
 330                 {
 331                         //                      BT_PROFILE("rayTestConcave");
 332                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
 333                         {
 334                                 ///optimized version for btBvhTriangleMeshShape
 335                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
 336                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
 337                                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
 338                                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
 339
 340                                 //ConvexCast::CastResult
 341                                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
 342                                 {
 343                                         btCollisionWorld::RayResultCallback* m_resultCallback;
 344                                         const btCollisionObject*        m_collisionObject;
 345                                         btTriangleMeshShape*    m_triangleMesh;
 346
 347                                         btTransform m_colObjWorldTransform;
 348
 349                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
 350                                                 btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape*      triangleMesh,const btTransform& colObjWorldTransform):
 351                                         //@BP Mod
 352                                         btTriangleRaycastCallback(from,to, resultCallback->m_flags),
 353                                                 m_resultCallback(resultCallback),
 354                                                 m_collisionObject(collisionObject),
 355                                                 m_triangleMesh(triangleMesh),
 356                                                 m_colObjWorldTransform(colObjWorldTransform)
 357                                         {
 358                                         }
 359
 360
 361                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
 362                                         {
 363                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
 364                                                 shapeInfo.m_shapePart = partId;
 365                                                 shapeInfo.m_triangleIndex = triangleIndex;
 366
 367                                                 btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 368
 369                                                 btCollisionWorld::LocalRayResult rayResult
 370                                                         (m_collisionObject,
 371                                                         &shapeInfo,
 372                                                         hitNormalWorld,
 373                                                         hitFraction);
 374
 375                                                 bool    normalInWorldSpace = true;
 376                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
 377                                         }
 378
 379                                 };
 380
 381                                 BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform);
 382                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 383                                 triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
 384                         } else
 385                         {
 386                                 //generic (slower) case
 387                                 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
 388
 389                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
 390
 391                                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
 392                                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
 393
 394                                 //ConvexCast::CastResult
 395
 396                                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
 397                                 {
 398                                         btCollisionWorld::RayResultCallback* m_resultCallback;
 399                                         const btCollisionObject*        m_collisionObject;
 400                                         btConcaveShape* m_triangleMesh;
 401
 402                                         btTransform m_colObjWorldTransform;
 403
 404                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
 405                                                 btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape*   triangleMesh, const btTransform& colObjWorldTransform):
 406                                         //@BP Mod
 407                                         btTriangleRaycastCallback(from,to, resultCallback->m_flags),
 408                                                 m_resultCallback(resultCallback),
 409                                                 m_collisionObject(collisionObject),
 410                                                 m_triangleMesh(triangleMesh),
 411                                                 m_colObjWorldTransform(colObjWorldTransform)
 412                                         {
 413                                         }
 414
 415
 416                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
 417                                         {
 418                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
 419                                                 shapeInfo.m_shapePart = partId;
 420                                                 shapeInfo.m_triangleIndex = triangleIndex;
 421
 422                                                 btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 423
 424                                                 btCollisionWorld::LocalRayResult rayResult
 425                                                         (m_collisionObject,
 426                                                         &shapeInfo,
 427                                                         hitNormalWorld,
 428                                                         hitFraction);
 429
 430                                                 bool    normalInWorldSpace = true;
 431                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
 432                                         }
 433
 434                                 };
 435
 436
 437                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
 438                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 439
 440                                 btVector3 rayAabbMinLocal = rayFromLocal;
 441                                 rayAabbMinLocal.setMin(rayToLocal);
 442                                 btVector3 rayAabbMaxLocal = rayFromLocal;
 443                                 rayAabbMaxLocal.setMax(rayToLocal);
 444
 445                                 concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
 446                         }
 447                 } else {
 448                         //                      BT_PROFILE("rayTestCompound");
 449                         if (collisionShape->isCompound())
 450                         {
 451                                 struct LocalInfoAdder2 : public RayResultCallback
 452                                 {
 453                                         RayResultCallback* m_userCallback;
 454                                         int m_i;
 455
 456                                         LocalInfoAdder2 (int i, RayResultCallback *user)
 457                                                 : m_userCallback(user), m_i(i)
 458                                         {
 459                                                 m_closestHitFraction = m_userCallback->m_closestHitFraction;
 460                                                 m_flags = m_userCallback->m_flags;
 461                                         }
 462                                         virtual bool needsCollision(btBroadphaseProxy* p) const
 463                                         {
 464                                                 return m_userCallback->needsCollision(p);
 465                                         }
 466
 467                                         virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
 468                                         {
 469                                                 btCollisionWorld::LocalShapeInfo shapeInfo;
 470                                                 shapeInfo.m_shapePart = -1;
 471                                                 shapeInfo.m_triangleIndex = m_i;
 472                                                 if (r.m_localShapeInfo == NULL)
 473                                                         r.m_localShapeInfo = &shapeInfo;
 474
 475                                                 const btScalar result = m_userCallback->addSingleResult(r, b);
 476                                                 m_closestHitFraction = m_userCallback->m_closestHitFraction;
 477                                                 return result;
 478                                         }
 479                                 };
 480
 481                                 struct RayTester : btDbvt::ICollide
 482                                 {
 483                                         const btCollisionObject* m_collisionObject;
 484                                         const btCompoundShape* m_compoundShape;
 485                                         const btTransform& m_colObjWorldTransform;
 486                                         const btTransform& m_rayFromTrans;
 487                                         const btTransform& m_rayToTrans;
 488                                         RayResultCallback& m_resultCallback;
 489
 490                                         RayTester(const btCollisionObject* collisionObject,
 491                                                         const btCompoundShape* compoundShape,
 492                                                         const btTransform& colObjWorldTransform,
 493                                                         const btTransform& rayFromTrans,
 494                                                         const btTransform& rayToTrans,
 495                                                         RayResultCallback& resultCallback):
 496                                                 m_collisionObject(collisionObject),
 497                                                 m_compoundShape(compoundShape),
 498                                                 m_colObjWorldTransform(colObjWorldTransform),
 499                                                 m_rayFromTrans(rayFromTrans),
 500                                                 m_rayToTrans(rayToTrans),
 501                                                 m_resultCallback(resultCallback)
 502                                         {
 503
 504                                         }
 505
 506                                         void ProcessLeaf(int i)
 507                                         {
 508                                                 const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
 509                                                 const btTransform& childTrans = m_compoundShape->getChildTransform(i);
 510                                                 btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
 511
 512                                                 btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans);
 513                                                 // replace collision shape so that callback can determine the triangle
 514
 515
 516
 517                                                 LocalInfoAdder2 my_cb(i, &m_resultCallback);
 518
 519                                                 rayTestSingleInternal(
 520                                                         m_rayFromTrans,
 521                                                         m_rayToTrans,
 522                                                         &tmpOb,
 523                                                         my_cb);
 524
 525                                         }
 526
 527                                         void Process(const btDbvtNode* leaf)
 528                                         {
 529                                                 ProcessLeaf(leaf->dataAsInt);
 530                                         }
 531                                 };
 532
 533                                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
 534                                 const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
 535
 536
 537                                 RayTester rayCB(
 538                                         collisionObjectWrap->getCollisionObject(),
 539                                         compoundShape,
 540                                         colObjWorldTransform,
 541                                         rayFromTrans,
 542                                         rayToTrans,
 543                                         resultCallback);
 544 #ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 545                                 if (dbvt)
 546                                 {
 547                                         btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
 548                                         btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
 549                                         btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
 550                                 }
 551                                 else
 552 #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 553                                 {
 554                                         for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
 555                                         {
 556                                                 rayCB.ProcessLeaf(i);
 557                                         }
 558                                 }
 559                         }
 560                 }
 561         }
 562 }
 563
 564 void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
 565                                                                                         btCollisionObject* collisionObject,
 566                                                                                         const btCollisionShape* collisionShape,
 567                                                                                         const btTransform& colObjWorldTransform,
 568                                                                                         ConvexResultCallback& resultCallback, btScalar allowedPenetration)
 569 {
 570         btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform);
 571         btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration);
 572 }
 573
 574 void    btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
 575                                                                                         const btCollisionObjectWrapper* colObjWrap,
 576                                                                                         ConvexResultCallback& resultCallback, btScalar allowedPenetration)
 577 {
 578         const btCollisionShape* collisionShape = colObjWrap->getCollisionShape();
 579         const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform();
 580
 581         if (collisionShape->isConvex())
 582         {
 583                 //BT_PROFILE("convexSweepConvex");
 584                 btConvexCast::CastResult castResult;
 585                 castResult.m_allowedPenetration = allowedPenetration;
 586                 castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
 587
 588                 btConvexShape* convexShape = (btConvexShape*) collisionShape;
 589                 btVoronoiSimplexSolver  simplexSolver;
 590                 btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
 591
 592                 btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
 593                 //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
 594                 //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
 595
 596                 btConvexCast* castPtr = &convexCaster1;
 597
 598
 599
 600                 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
 601                 {
 602                         //add hit
 603                         if (castResult.m_normal.length2() > btScalar(0.0001))
 604                         {
 605                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 606                                 {
 607                                         castResult.m_normal.normalize();
 608                                         btCollisionWorld::LocalConvexResult localConvexResult
 609                                                 (
 610                                                 colObjWrap->getCollisionObject(),
 611                                                 0,
 612                                                 castResult.m_normal,
 613                                                 castResult.m_hitPoint,
 614                                                 castResult.m_fraction
 615                                                 );
 616
 617                                         bool normalInWorldSpace = true;
 618                                         resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
 619
 620                                 }
 621                         }
 622                 }
 623         } else {
 624                 if (collisionShape->isConcave())
 625                 {
 626                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
 627                         {
 628                                 //BT_PROFILE("convexSweepbtBvhTriangleMesh");
 629                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
 630                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
 631                                 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
 632                                 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
 633                                 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
 634                                 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
 635
 636                                 //ConvexCast::CastResult
 637                                 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
 638                                 {
 639                                         btCollisionWorld::ConvexResultCallback* m_resultCallback;
 640                                         const btCollisionObject*        m_collisionObject;
 641                                         btTriangleMeshShape*    m_triangleMesh;
 642
 643                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
 644                                                 btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape*   triangleMesh, const btTransform& triangleToWorld):
 645                                         btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
 646                                                 m_resultCallback(resultCallback),
 647                                                 m_collisionObject(collisionObject),
 648                                                 m_triangleMesh(triangleMesh)
 649                                         {
 650                                         }
 651
 652
 653                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
 654                                         {
 655                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
 656                                                 shapeInfo.m_shapePart = partId;
 657                                                 shapeInfo.m_triangleIndex = triangleIndex;
 658                                                 if (hitFraction <= m_resultCallback->m_closestHitFraction)
 659                                                 {
 660
 661                                                         btCollisionWorld::LocalConvexResult convexResult
 662                                                                 (m_collisionObject,
 663                                                                 &shapeInfo,
 664                                                                 hitNormalLocal,
 665                                                                 hitPointLocal,
 666                                                                 hitFraction);
 667
 668                                                         bool    normalInWorldSpace = true;
 669
 670
 671                                                         return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
 672                                                 }
 673                                                 return hitFraction;
 674                                         }
 675
 676                                 };
 677
 678                                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform);
 679                                 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
 680                                 tccb.m_allowedPenetration = allowedPenetration;
 681                                 btVector3 boxMinLocal, boxMaxLocal;
 682                                 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
 683                                 triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
 684                         } else
 685                         {
 686                                 if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
 687                                 {
 688                                         btConvexCast::CastResult castResult;
 689                                         castResult.m_allowedPenetration = allowedPenetration;
 690                                         castResult.m_fraction = resultCallback.m_closestHitFraction;
 691                                         btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
 692                                         btContinuousConvexCollision convexCaster1(castShape,planeShape);
 693                                         btConvexCast* castPtr = &convexCaster1;
 694
 695                                         if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
 696                                         {
 697                                                 //add hit
 698                                                 if (castResult.m_normal.length2() > btScalar(0.0001))
 699                                                 {
 700                                                         if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 701                                                         {
 702                                                                 castResult.m_normal.normalize();
 703                                                                 btCollisionWorld::LocalConvexResult localConvexResult
 704                                                                         (
 705                                                                         colObjWrap->getCollisionObject(),
 706                                                                         0,
 707                                                                         castResult.m_normal,
 708                                                                         castResult.m_hitPoint,
 709                                                                         castResult.m_fraction
 710                                                                         );
 711
 712                                                                 bool normalInWorldSpace = true;
 713                                                                 resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
 714                                                         }
 715                                                 }
 716                                         }
 717
 718                                 } else
 719                                 {
 720                                         //BT_PROFILE("convexSweepConcave");
 721                                         btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
 722                                         btTransform worldTocollisionObject = colObjWorldTransform.inverse();
 723                                         btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
 724                                         btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
 725                                         // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
 726                                         btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
 727
 728                                         //ConvexCast::CastResult
 729                                         struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
 730                                         {
 731                                                 btCollisionWorld::ConvexResultCallback* m_resultCallback;
 732                                                 const btCollisionObject*        m_collisionObject;
 733                                                 btConcaveShape* m_triangleMesh;
 734
 735                                                 BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
 736                                                         btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape*        triangleMesh, const btTransform& triangleToWorld):
 737                                                 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
 738                                                         m_resultCallback(resultCallback),
 739                                                         m_collisionObject(collisionObject),
 740                                                         m_triangleMesh(triangleMesh)
 741                                                 {
 742                                                 }
 743
 744
 745                                                 virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
 746                                                 {
 747                                                         btCollisionWorld::LocalShapeInfo        shapeInfo;
 748                                                         shapeInfo.m_shapePart = partId;
 749                                                         shapeInfo.m_triangleIndex = triangleIndex;
 750                                                         if (hitFraction <= m_resultCallback->m_closestHitFraction)
 751                                                         {
 752
 753                                                                 btCollisionWorld::LocalConvexResult convexResult
 754                                                                         (m_collisionObject,
 755                                                                         &shapeInfo,
 756                                                                         hitNormalLocal,
 757                                                                         hitPointLocal,
 758                                                                         hitFraction);
 759
 760                                                                 bool    normalInWorldSpace = false;
 761
 762                                                                 return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
 763                                                         }
 764                                                         return hitFraction;
 765                                                 }
 766
 767                                         };
 768
 769                                         BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
 770                                         tccb.m_hitFraction = resultCallback.m_closestHitFraction;
 771                                         tccb.m_allowedPenetration = allowedPenetration;
 772                                         btVector3 boxMinLocal, boxMaxLocal;
 773                                         castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
 774
 775                                         btVector3 rayAabbMinLocal = convexFromLocal;
 776                                         rayAabbMinLocal.setMin(convexToLocal);
 777                                         btVector3 rayAabbMaxLocal = convexFromLocal;
 778                                         rayAabbMaxLocal.setMax(convexToLocal);
 779                                         rayAabbMinLocal += boxMinLocal;
 780                                         rayAabbMaxLocal += boxMaxLocal;
 781                                         concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
 782                                 }
 783                         }
 784                 } else {
 785                         ///@todo : use AABB tree or other BVH acceleration structure!
 786                         if (collisionShape->isCompound())
 787                         {
 788                                 BT_PROFILE("convexSweepCompound");
 789                                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
 790                                 int i=0;
 791                                 for (i=0;i<compoundShape->getNumChildShapes();i++)
 792                                 {
 793                                         btTransform childTrans = compoundShape->getChildTransform(i);
 794                                         const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
 795                                         btTransform childWorldTrans = colObjWorldTransform * childTrans;
 796
 797                     struct      LocalInfoAdder : public ConvexResultCallback {
 798                             ConvexResultCallback* m_userCallback;
 799                                                         int m_i;
 800
 801                             LocalInfoAdder (int i, ConvexResultCallback *user)
 802                                                                 : m_userCallback(user), m_i(i)
 803                                                         {
 804                                                                 m_closestHitFraction = m_userCallback->m_closestHitFraction;
 805                                                         }
 806                                                         virtual bool needsCollision(btBroadphaseProxy* p) const
 807                                                         {
 808                                                                 return m_userCallback->needsCollision(p);
 809                                                         }
 810                             virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&      r,      bool b)
 811                             {
 812                                     btCollisionWorld::LocalShapeInfo    shapeInfo;
 813                                     shapeInfo.m_shapePart = -1;
 814                                     shapeInfo.m_triangleIndex = m_i;
 815                                     if (r.m_localShapeInfo == NULL)
 816                                         r.m_localShapeInfo = &shapeInfo;
 817                                                                         const btScalar result = m_userCallback->addSingleResult(r, b);
 818                                                                         m_closestHitFraction = m_userCallback->m_closestHitFraction;
 819                                                                         return result;
 820
 821                             }
 822                     };
 823
 824                     LocalInfoAdder my_cb(i, &resultCallback);
 825
 826                                         btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans);
 827
 828                                         objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans,
 829                                                 &tmpObj,my_cb, allowedPenetration);
 830
 831                                 }
 832                         }
 833                 }
 834         }
 835 }
 836
 837
 838 struct btSingleRayCallback : public btBroadphaseRayCallback
 839 {
 840
 841         btVector3       m_rayFromWorld;
 842         btVector3       m_rayToWorld;
 843         btTransform     m_rayFromTrans;
 844         btTransform     m_rayToTrans;
 845         btVector3       m_hitNormal;
 846
 847         const btCollisionWorld* m_world;
 848         btCollisionWorld::RayResultCallback&    m_resultCallback;
 849
 850         btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
 851                 :m_rayFromWorld(rayFromWorld),
 852                 m_rayToWorld(rayToWorld),
 853                 m_world(world),
 854                 m_resultCallback(resultCallback)
 855         {
 856                 m_rayFromTrans.setIdentity();
 857                 m_rayFromTrans.setOrigin(m_rayFromWorld);
 858                 m_rayToTrans.setIdentity();
 859                 m_rayToTrans.setOrigin(m_rayToWorld);
 860
 861                 btVector3 rayDir = (rayToWorld-rayFromWorld);
 862
 863                 rayDir.normalize ();
 864                 ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
 865                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
 866                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
 867                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
 868                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
 869                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 870                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 871
 872                 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
 873
 874         }
 875
 876
 877
 878         virtual bool    process(const btBroadphaseProxy* proxy)
 879         {
 880                 ///terminate further ray tests, once the closestHitFraction reached zero
 881                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 882                         return false;
 883
 884                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
 885
 886                 //only perform raycast if filterMask matches
 887                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 888                 {
 889                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 890                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 891 #if 0
 892 #ifdef RECALCULATE_AABB
 893                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 894                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
 895 #else
 896                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
 897                         const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
 898                         const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
 899 #endif
 900 #endif
 901                         //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
 902                         //culling already done by broadphase
 903                         //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
 904                         {
 905                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
 906                                         collisionObject,
 907                                         collisionObject->getCollisionShape(),
 908                                         collisionObject->getWorldTransform(),
 909                                         m_resultCallback);
 910                         }
 911                 }
 912                 return true;
 913         }
 914 };
 915
 916 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 917 {
 918         //BT_PROFILE("rayTest");
 919         /// use the broadphase to accelerate the search for objects, based on their aabb
 920         /// and for each object with ray-aabb overlap, perform an exact ray test
 921         btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
 922
 923 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
 924         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
 925 #else
 926         for (int i=0;i<this->getNumCollisionObjects();i++)
 927         {
 928                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
 929         }
 930 #endif //USE_BRUTEFORCE_RAYBROADPHASE
 931
 932 }
 933
 934
 935 struct btSingleSweepCallback : public btBroadphaseRayCallback
 936 {
 937
 938         btTransform     m_convexFromTrans;
 939         btTransform     m_convexToTrans;
 940         btVector3       m_hitNormal;
 941         const btCollisionWorld* m_world;
 942         btCollisionWorld::ConvexResultCallback& m_resultCallback;
 943         btScalar        m_allowedCcdPenetration;
 944         const btConvexShape* m_castShape;
 945
 946
 947         btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
 948                 :m_convexFromTrans(convexFromTrans),
 949                 m_convexToTrans(convexToTrans),
 950                 m_world(world),
 951                 m_resultCallback(resultCallback),
 952                 m_allowedCcdPenetration(allowedPenetration),
 953                 m_castShape(castShape)
 954         {
 955                 btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
 956                 btVector3 rayDir = unnormalizedRayDir.normalized();
 957                 ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
 958                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
 959                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
 960                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
 961                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
 962                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 963                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 964
 965                 m_lambda_max = rayDir.dot(unnormalizedRayDir);
 966
 967         }
 968
 969         virtual bool    process(const btBroadphaseProxy* proxy)
 970         {
 971                 ///terminate further convex sweep tests, once the closestHitFraction reached zero
 972                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 973                         return false;
 974
 975                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
 976
 977                 //only perform raycast if filterMask matches
 978                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
 979                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 980                         m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
 981                                 collisionObject,
 982                                 collisionObject->getCollisionShape(),
 983                                 collisionObject->getWorldTransform(),
 984                                 m_resultCallback,
 985                                 m_allowedCcdPenetration);
 986                 }
 987
 988                 return true;
 989         }
 990 };
 991
 992
 993
 994 void    btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
 995 {
 996
 997         BT_PROFILE("convexSweepTest");
 998         /// use the broadphase to accelerate the search for objects, based on their aabb
 999         /// and for each object with ray-aabb overlap, perform an exact ray test
1000         /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
1001
1002
1003
1004         btTransform     convexFromTrans,convexToTrans;
1005         convexFromTrans = convexFromWorld;
1006         convexToTrans = convexToWorld;
1007         btVector3 castShapeAabbMin, castShapeAabbMax;
1008         /* Compute AABB that encompasses angular movement */
1009         {
1010                 btVector3 linVel, angVel;
1011                 btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel);
1012                 btVector3 zeroLinVel;
1013                 zeroLinVel.setValue(0,0,0);
1014                 btTransform R;
1015                 R.setIdentity ();
1016                 R.setRotation (convexFromTrans.getRotation());
1017                 castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax);
1018         }
1019
1020 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
1021
1022         btSingleSweepCallback   convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
1023
1024         m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
1025
1026 #else
1027         /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
1028         // do a ray-shape query using convexCaster (CCD)
1029         int i;
1030         for (i=0;i<m_collisionObjects.size();i++)
1031         {
1032                 btCollisionObject*      collisionObject= m_collisionObjects[i];
1033                 //only perform raycast if filterMask matches
1034                 if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
1035                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
1036                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
1037                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
1038                         AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
1039                         btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
1040                         btVector3 hitNormal;
1041                         if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
1042                         {
1043                                 objectQuerySingle(castShape, convexFromTrans,convexToTrans,
1044                                         collisionObject,
1045                                         collisionObject->getCollisionShape(),
1046                                         collisionObject->getWorldTransform(),
1047                                         resultCallback,
1048                                         allowedCcdPenetration);
1049                         }
1050                 }
1051         }
1052 #endif //USE_BRUTEFORCE_RAYBROADPHASE
1053 }
1054
1055
1056
1057 struct btBridgedManifoldResult : public btManifoldResult
1058 {
1059
1060         btCollisionWorld::ContactResultCallback&        m_resultCallback;
1061
1062         btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback )
1063                 :btManifoldResult(obj0Wrap,obj1Wrap),
1064                 m_resultCallback(resultCallback)
1065         {
1066         }
1067
1068         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
1069         {
1070                 bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
1071                 btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
1072                 btVector3 localA;
1073                 btVector3 localB;
1074                 if (isSwapped)
1075                 {
1076                         localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
1077                         localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1078                 } else
1079                 {
1080                         localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
1081                         localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1082                 }
1083
1084                 btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
1085                 newPt.m_positionWorldOnA = pointA;
1086                 newPt.m_positionWorldOnB = pointInWorld;
1087
1088            //BP mod, store contact triangles.
1089                 if (isSwapped)
1090                 {
1091                         newPt.m_partId0 = m_partId1;
1092                         newPt.m_partId1 = m_partId0;
1093                         newPt.m_index0  = m_index1;
1094                         newPt.m_index1  = m_index0;
1095                 } else
1096                 {
1097                         newPt.m_partId0 = m_partId0;
1098                         newPt.m_partId1 = m_partId1;
1099                         newPt.m_index0  = m_index0;
1100                         newPt.m_index1  = m_index1;
1101                 }
1102
1103                 //experimental feature info, for per-triangle material etc.
1104                 const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
1105                 const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
1106                 m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
1107
1108         }
1109
1110 };
1111
1112
1113
1114 struct btSingleContactCallback : public btBroadphaseAabbCallback
1115 {
1116
1117         btCollisionObject* m_collisionObject;
1118         btCollisionWorld*       m_world;
1119         btCollisionWorld::ContactResultCallback&        m_resultCallback;
1120
1121
1122         btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
1123                 :m_collisionObject(collisionObject),
1124                 m_world(world),
1125                 m_resultCallback(resultCallback)
1126         {
1127         }
1128
1129         virtual bool    process(const btBroadphaseProxy* proxy)
1130         {
1131                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
1132                 if (collisionObject == m_collisionObject)
1133                         return true;
1134
1135                 //only perform raycast if filterMask matches
1136                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1137                 {
1138                         btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform());
1139                         btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform());
1140
1141                         btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
1142                         if (algorithm)
1143                         {
1144                                 btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
1145                                 //discrete collision detection query
1146
1147                                 algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult);
1148
1149                                 algorithm->~btCollisionAlgorithm();
1150                                 m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
1151                         }
1152                 }
1153                 return true;
1154         }
1155 };
1156
1157
1158 ///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
1159 ///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
1160 void    btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
1161 {
1162         btVector3 aabbMin,aabbMax;
1163         colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
1164         btSingleContactCallback contactCB(colObj,this,resultCallback);
1165
1166         m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
1167 }
1168
1169
1170 ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
1171 ///it reports one or more contact points (including the one with deepest penetration)
1172 void    btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
1173 {
1174         btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform());
1175         btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform());
1176
1177         btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
1178         if (algorithm)
1179         {
1180                 btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
1181                 //discrete collision detection query
1182                 algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
1183
1184                 algorithm->~btCollisionAlgorithm();
1185                 getDispatcher()->freeCollisionAlgorithm(algorithm);
1186         }
1187
1188 }
1189
1190
1191
1192
1193 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
1194 {
1195         btIDebugDraw*   m_debugDrawer;
1196         btVector3       m_color;
1197         btTransform     m_worldTrans;
1198
1199 public:
1200
1201         DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
1202           m_debugDrawer(debugDrawer),
1203                   m_color(color),
1204                   m_worldTrans(worldTrans)
1205           {
1206           }
1207
1208           virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
1209           {
1210                   processTriangle(triangle,partId,triangleIndex);
1211           }
1212
1213           virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
1214           {
1215                   (void)partId;
1216                   (void)triangleIndex;
1217
1218                   btVector3 wv0,wv1,wv2;
1219                   wv0 = m_worldTrans*triangle[0];
1220                   wv1 = m_worldTrans*triangle[1];
1221                   wv2 = m_worldTrans*triangle[2];
1222                   btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
1223
1224           if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals )
1225           {
1226                     btVector3 normal = (wv1-wv0).cross(wv2-wv0);
1227                     normal.normalize();
1228                     btVector3 normalColor(1,1,0);
1229                     m_debugDrawer->drawLine(center,center+normal,normalColor);
1230           }
1231                   m_debugDrawer->drawLine(wv0,wv1,m_color);
1232                   m_debugDrawer->drawLine(wv1,wv2,m_color);
1233                   m_debugDrawer->drawLine(wv2,wv0,m_color);
1234           }
1235 };
1236
1237
1238 void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1239 {
1240         // Draw a small simplex at the center of the object
1241         getDebugDrawer()->drawTransform(worldTransform,1);
1242
1243         if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1244         {
1245                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1246                 for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
1247                 {
1248                         btTransform childTrans = compoundShape->getChildTransform(i);
1249                         const btCollisionShape* colShape = compoundShape->getChildShape(i);
1250                         debugDrawObject(worldTransform*childTrans,colShape,color);
1251                 }
1252
1253         } else
1254         {
1255
1256         switch (shape->getShapeType())
1257         {
1258
1259         case BOX_SHAPE_PROXYTYPE:
1260             {
1261                 const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
1262                 btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
1263                 getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
1264                 break;
1265             }
1266
1267         case SPHERE_SHAPE_PROXYTYPE:
1268             {
1269                 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1270                 btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1271
1272                 getDebugDrawer()->drawSphere(radius, worldTransform, color);
1273                 break;
1274             }
1275         case MULTI_SPHERE_SHAPE_PROXYTYPE:
1276             {
1277                 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1278
1279                 btTransform childTransform;
1280                 childTransform.setIdentity();
1281
1282                 for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1283                 {
1284                     childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
1285                     getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
1286                 }
1287
1288                 break;
1289             }
1290         case CAPSULE_SHAPE_PROXYTYPE:
1291             {
1292                 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1293
1294                 btScalar radius = capsuleShape->getRadius();
1295                 btScalar halfHeight = capsuleShape->getHalfHeight();
1296
1297                 int upAxis = capsuleShape->getUpAxis();
1298                 getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
1299                 break;
1300             }
1301         case CONE_SHAPE_PROXYTYPE:
1302             {
1303                 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1304                 btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1305                 btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1306
1307                 int upAxis= coneShape->getConeUpIndex();
1308                 getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
1309                 break;
1310
1311             }
1312         case CYLINDER_SHAPE_PROXYTYPE:
1313             {
1314                 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1315                 int upAxis = cylinder->getUpAxis();
1316                 btScalar radius = cylinder->getRadius();
1317                 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1318                 getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
1319                 break;
1320             }
1321
1322         case STATIC_PLANE_PROXYTYPE:
1323             {
1324                 const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1325                 btScalar planeConst = staticPlaneShape->getPlaneConstant();
1326                 const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1327                 getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
1328                 break;
1329
1330             }
1331         default:
1332             {
1333
1334                 /// for polyhedral shapes
1335                 if (shape->isPolyhedral())
1336                 {
1337                     btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1338
1339                     int i;
1340                     if (polyshape->getConvexPolyhedron())
1341                     {
1342                         const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
1343                         for (i=0;i<poly->m_faces.size();i++)
1344                         {
1345                             btVector3 centroid(0,0,0);
1346                             int numVerts = poly->m_faces[i].m_indices.size();
1347                             if (numVerts)
1348                             {
1349                                 int lastV = poly->m_faces[i].m_indices[numVerts-1];
1350                                 for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
1351                                 {
1352                                     int curVert = poly->m_faces[i].m_indices[v];
1353                                     centroid+=poly->m_vertices[curVert];
1354                                     getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
1355                                     lastV = curVert;
1356                                 }
1357                             }
1358                             centroid*= btScalar(1.f)/btScalar(numVerts);
1359                             if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
1360                             {
1361                                 btVector3 normalColor(1,1,0);
1362                                 btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
1363                                 getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
1364                             }
1365
1366                         }
1367
1368
1369                     } else
1370                     {
1371                         for (i=0;i<polyshape->getNumEdges();i++)
1372                         {
1373                             btVector3 a,b;
1374                             polyshape->getEdge(i,a,b);
1375                             btVector3 wa = worldTransform * a;
1376                             btVector3 wb = worldTransform * b;
1377                             getDebugDrawer()->drawLine(wa,wb,color);
1378                         }
1379                     }
1380
1381
1382                 }
1383
1384                 if (shape->isConcave())
1385                 {
1386                     btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1387
1388                     ///@todo pass camera, for some culling? no -> we are not a graphics lib
1389                     btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1390                     btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1391
1392                     DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1393                     concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1394
1395                 }
1396
1397                 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1398                 {
1399                     btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1400                     //todo: pass camera for some culling
1401                     btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1402                     btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1403                     //DebugDrawcallback drawCallback;
1404                     DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1405                     convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1406                 }
1407
1408
1409
1410             }
1411
1412                 }
1413         }
1414 }
1415
1416
1417 void    btCollisionWorld::debugDrawWorld()
1418 {
1419         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
1420         {
1421                 int numManifolds = getDispatcher()->getNumManifolds();
1422                 btVector3 color(1,1,0);
1423                 for (int i=0;i<numManifolds;i++)
1424                 {
1425                         btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
1426                         //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
1427                         //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
1428
1429                         int numContacts = contactManifold->getNumContacts();
1430                         for (int j=0;j<numContacts;j++)
1431                         {
1432                                 btManifoldPoint& cp = contactManifold->getContactPoint(j);
1433                                 getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1434                         }
1435                 }
1436         }
1437
1438         if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
1439         {
1440                 int i;
1441
1442                 for (  i=0;i<m_collisionObjects.size();i++)
1443                 {
1444                         btCollisionObject* colObj = m_collisionObjects[i];
1445                         if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
1446                         {
1447                                 if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
1448                                 {
1449                                         btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
1450                                         switch(colObj->getActivationState())
1451                                         {
1452                                         case  ACTIVE_TAG:
1453                                                 color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
1454                                         case ISLAND_SLEEPING:
1455                                                 color =  btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
1456                                         case WANTS_DEACTIVATION:
1457                                                 color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
1458                                         case DISABLE_DEACTIVATION:
1459                                                 color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
1460                                         case DISABLE_SIMULATION:
1461                                                 color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
1462                                         default:
1463                                                 {
1464                                                         color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
1465                                                 }
1466                                         };
1467
1468                                         debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
1469                                 }
1470                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
1471                                 {
1472                                         btVector3 minAabb,maxAabb;
1473                                         btVector3 colorvec(1,0,0);
1474                                         colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
1475                                         btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
1476                                         minAabb -= contactThreshold;
1477                                         maxAabb += contactThreshold;
1478
1479                                         btVector3 minAabb2,maxAabb2;
1480
1481                                         if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
1482                                         {
1483                                                 colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
1484                                                 minAabb2 -= contactThreshold;
1485                                                 maxAabb2 += contactThreshold;
1486                                                 minAabb.setMin(minAabb2);
1487                                                 maxAabb.setMax(maxAabb2);
1488                                         }
1489
1490                                         m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
1491                                 }
1492                         }
1493
1494                 }
1495         }
1496 }
1497
1498
1499 void    btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
1500 {
1501         int i;
1502         //serialize all collision objects
1503         for (i=0;i<m_collisionObjects.size();i++)
1504         {
1505                 btCollisionObject* colObj = m_collisionObjects[i];
1506                 if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
1507                 {
1508                         colObj->serializeSingleObject(serializer);
1509                 }
1510         }
1511
1512         ///keep track of shapes already serialized
1513         btHashMap<btHashPtr,btCollisionShape*>  serializedShapes;
1514
1515         for (i=0;i<m_collisionObjects.size();i++)
1516         {
1517                 btCollisionObject* colObj = m_collisionObjects[i];
1518                 btCollisionShape* shape = colObj->getCollisionShape();
1519
1520                 if (!serializedShapes.find(shape))
1521                 {
1522                         serializedShapes.insert(shape,shape);
1523                         shape->serializeSingleShape(serializer);
1524                 }
1525         }
1526
1527 }
1528
1529
1530 void    btCollisionWorld::serialize(btSerializer* serializer)
1531 {
1532
1533         serializer->startSerialization();
1534
1535         serializeCollisionObjects(serializer);
1536
1537         serializer->finishSerialization();
1538 }
1539