| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| |
| |
| #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" |
| |
| |
| #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" |
| |
| #include "BulletCollision/CollisionShapes/btCollisionShape.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionObject.h" |
| #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" |
| #include "LinearMath/btPoolAllocator.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" |
| |
| int gNumManifold = 0; |
| |
| #ifdef BT_DEBUG |
| #include <stdio.h> |
| #endif |
| |
| |
| btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): |
| m_count(0), |
| m_useIslands(true), |
| m_staticWarningReported(false), |
| m_collisionConfiguration(collisionConfiguration) |
| { |
| int i; |
| |
| setNearCallback(defaultNearCallback); |
| |
| m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); |
| |
| m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); |
| |
| for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) |
| { |
| for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) |
| { |
| m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); |
| btAssert(m_doubleDispatch[i][j]); |
| } |
| } |
| |
| |
| } |
| |
| |
| void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) |
| { |
| m_doubleDispatch[proxyType0][proxyType1] = createFunc; |
| } |
| |
| btCollisionDispatcher::~btCollisionDispatcher() |
| { |
| } |
| |
| btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) |
| { |
| gNumManifold++; |
| |
| //btAssert(gNumManifold < 65535); |
| |
| |
| btCollisionObject* body0 = (btCollisionObject*)b0; |
| btCollisionObject* body1 = (btCollisionObject*)b1; |
| |
| //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold' |
| //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold())); |
| btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()); |
| |
| btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); |
| |
| void* mem = 0; |
| |
| if (m_persistentManifoldPoolAllocator->getFreeCount()) |
| { |
| mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); |
| } else |
| { |
| mem = btAlignedAlloc(sizeof(btPersistentManifold),16); |
| |
| } |
| btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); |
| manifold->m_index1a = m_manifoldsPtr.size(); |
| m_manifoldsPtr.push_back(manifold); |
| |
| return manifold; |
| } |
| |
| void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) |
| { |
| manifold->clearManifold(); |
| } |
| |
| |
| void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) |
| { |
| |
| gNumManifold--; |
| |
| //printf("releaseManifold: gNumManifold %d\n",gNumManifold); |
| clearManifold(manifold); |
| |
| int findIndex = manifold->m_index1a; |
| btAssert(findIndex < m_manifoldsPtr.size()); |
| m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); |
| m_manifoldsPtr[findIndex]->m_index1a = findIndex; |
| m_manifoldsPtr.pop_back(); |
| |
| manifold->~btPersistentManifold(); |
| if (m_persistentManifoldPoolAllocator->validPtr(manifold)) |
| { |
| m_persistentManifoldPoolAllocator->freeMemory(manifold); |
| } else |
| { |
| btAlignedFree(manifold); |
| } |
| |
| } |
| |
| |
| |
| btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold) |
| { |
| |
| btCollisionAlgorithmConstructionInfo ci; |
| |
| ci.m_dispatcher1 = this; |
| ci.m_manifold = sharedManifold; |
| btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0,body1); |
| |
| return algo; |
| } |
| |
| |
| |
| |
| bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1) |
| { |
| //here you can do filtering |
| bool hasResponse = |
| (body0->hasContactResponse() && body1->hasContactResponse()); |
| //no response between two static/kinematic bodies: |
| hasResponse = hasResponse && |
| ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject())); |
| return hasResponse; |
| } |
| |
| bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1) |
| { |
| btAssert(body0); |
| btAssert(body1); |
| |
| bool needsCollision = true; |
| |
| #ifdef BT_DEBUG |
| if (!m_staticWarningReported) |
| { |
| //broadphase filtering already deals with this |
| if ((body0->isStaticObject() || body0->isKinematicObject()) && |
| (body1->isStaticObject() || body1->isKinematicObject())) |
| { |
| m_staticWarningReported = true; |
| printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); |
| } |
| } |
| #endif //BT_DEBUG |
| |
| if ((!body0->isActive()) && (!body1->isActive())) |
| needsCollision = false; |
| else if (!body0->checkCollideWith(body1)) |
| needsCollision = false; |
| |
| return needsCollision ; |
| |
| } |
| |
| |
| |
| ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) |
| ///this is useful for the collision dispatcher. |
| class btCollisionPairCallback : public btOverlapCallback |
| { |
| const btDispatcherInfo& m_dispatchInfo; |
| btCollisionDispatcher* m_dispatcher; |
| |
| public: |
| |
| btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) |
| :m_dispatchInfo(dispatchInfo), |
| m_dispatcher(dispatcher) |
| { |
| } |
| |
| /*btCollisionPairCallback& operator=(btCollisionPairCallback& other) |
| { |
| m_dispatchInfo = other.m_dispatchInfo; |
| m_dispatcher = other.m_dispatcher; |
| return *this; |
| } |
| */ |
| |
| |
| virtual ~btCollisionPairCallback() {} |
| |
| |
| virtual bool processOverlap(btBroadphasePair& pair) |
| { |
| (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); |
| |
| return false; |
| } |
| }; |
| |
| |
| |
| void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) |
| { |
| //m_blockedForChanges = true; |
| |
| btCollisionPairCallback collisionCallback(dispatchInfo,this); |
| |
| pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); |
| |
| //m_blockedForChanges = false; |
| |
| } |
| |
| |
| |
| |
| //by default, Bullet will use this near callback |
| void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) |
| { |
| btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; |
| btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; |
| |
| if (dispatcher.needsCollision(colObj0,colObj1)) |
| { |
| //dispatcher will keep algorithms persistent in the collision pair |
| if (!collisionPair.m_algorithm) |
| { |
| collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1); |
| } |
| |
| if (collisionPair.m_algorithm) |
| { |
| btManifoldResult contactPointResult(colObj0,colObj1); |
| |
| if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) |
| { |
| //discrete collision detection query |
| collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult); |
| } else |
| { |
| //continuous collision detection query, time of impact (toi) |
| btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); |
| if (dispatchInfo.m_timeOfImpact > toi) |
| dispatchInfo.m_timeOfImpact = toi; |
| |
| } |
| } |
| } |
| |
| } |
| |
| |
| void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) |
| { |
| if (m_collisionAlgorithmPoolAllocator->getFreeCount()) |
| { |
| return m_collisionAlgorithmPoolAllocator->allocate(size); |
| } |
| |
| //warn user for overflow? |
| return btAlignedAlloc(static_cast<size_t>(size), 16); |
| } |
| |
| void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) |
| { |
| if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) |
| { |
| m_collisionAlgorithmPoolAllocator->freeMemory(ptr); |
| } else |
| { |
| btAlignedFree(ptr); |
| } |
| } |