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