dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

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