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