| /* | |
| 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/BroadphaseCollision/btMultiSapBroadphase.h" | |
| #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" | |
| #include "LinearMath/btAabbUtil2.h" | |
| #include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" | |
| /// btSapBroadphaseArray m_sapBroadphases; | |
| /// btOverlappingPairCache* m_overlappingPairs; | |
| extern int gOverlappingPairs; | |
| /* | |
| class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache | |
| { | |
| public: | |
| virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) | |
| { | |
| return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); | |
| } | |
| }; | |
| */ | |
| btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) | |
| :m_overlappingPairs(pairCache), | |
| m_optimizedAabbTree(0), | |
| m_ownsPairCache(false), | |
| m_invalidPair(0) | |
| { | |
| if (!m_overlappingPairs) | |
| { | |
| m_ownsPairCache = true; | |
| void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); | |
| m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); | |
| } | |
| struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback | |
| { | |
| virtual ~btMultiSapOverlapFilterCallback() | |
| {} | |
| // return true when pairs need collision | |
| virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const | |
| { | |
| btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; | |
| btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; | |
| bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; | |
| collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); | |
| return collides; | |
| } | |
| }; | |
| void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); | |
| m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); | |
| m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); | |
| // mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); | |
| // m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); | |
| } | |
| btMultiSapBroadphase::~btMultiSapBroadphase() | |
| { | |
| if (m_ownsPairCache) | |
| { | |
| m_overlappingPairs->~btOverlappingPairCache(); | |
| btAlignedFree(m_overlappingPairs); | |
| } | |
| } | |
| void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) | |
| { | |
| m_optimizedAabbTree = new btQuantizedBvh(); | |
| m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); | |
| QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); | |
| for (int i=0;i<m_sapBroadphases.size();i++) | |
| { | |
| btQuantizedBvhNode node; | |
| btVector3 aabbMin,aabbMax; | |
| m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax); | |
| m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); | |
| m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); | |
| int partId = 0; | |
| node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; | |
| nodes.push_back(node); | |
| } | |
| m_optimizedAabbTree->buildInternal(); | |
| } | |
| btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/) | |
| { | |
| //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested | |
| void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16); | |
| btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask); | |
| m_multiSapProxies.push_back(proxy); | |
| ///this should deal with inserting/removal into child broadphases | |
| setAabb(proxy,aabbMin,aabbMax,dispatcher); | |
| return proxy; | |
| } | |
| void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) | |
| { | |
| ///not yet | |
| btAssert(0); | |
| } | |
| void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase) | |
| { | |
| void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16); | |
| btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy; | |
| bridgeProxyRef->m_childProxy = childProxy; | |
| bridgeProxyRef->m_childBroadphase = childBroadphase; | |
| parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef); | |
| } | |
| bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax); | |
| bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax) | |
| { | |
| return | |
| amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() && | |
| amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() && | |
| amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); | |
| } | |
| void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const | |
| { | |
| btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy); | |
| aabbMin = multiProxy->m_aabbMin; | |
| aabbMax = multiProxy->m_aabbMax; | |
| } | |
| void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) | |
| { | |
| for (int i=0;i<m_multiSapProxies.size();i++) | |
| { | |
| rayCallback.process(m_multiSapProxies[i]); | |
| } | |
| } | |
| //#include <stdio.h> | |
| void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) | |
| { | |
| btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy); | |
| multiProxy->m_aabbMin = aabbMin; | |
| multiProxy->m_aabbMax = aabbMax; | |
| // bool fullyContained = false; | |
| // bool alreadyInSimple = false; | |
| struct MyNodeOverlapCallback : public btNodeOverlapCallback | |
| { | |
| btMultiSapBroadphase* m_multiSap; | |
| btMultiSapProxy* m_multiProxy; | |
| btDispatcher* m_dispatcher; | |
| MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) | |
| :m_multiSap(multiSap), | |
| m_multiProxy(multiProxy), | |
| m_dispatcher(dispatcher) | |
| { | |
| } | |
| virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) | |
| { | |
| btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; | |
| int containingBroadphaseIndex = -1; | |
| //already found? | |
| for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++) | |
| { | |
| if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) | |
| { | |
| containingBroadphaseIndex = i; | |
| break; | |
| } | |
| } | |
| if (containingBroadphaseIndex<0) | |
| { | |
| //add it | |
| btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy); | |
| m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase); | |
| } | |
| } | |
| }; | |
| MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); | |
| if (m_optimizedAabbTree) | |
| m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); | |
| int i; | |
| for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) | |
| { | |
| btVector3 worldAabbMin,worldAabbMax; | |
| multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); | |
| bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); | |
| if (!overlapsBroadphase) | |
| { | |
| //remove it now | |
| btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; | |
| btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; | |
| bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); | |
| multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); | |
| multiProxy->m_bridgeProxies.pop_back(); | |
| } | |
| } | |
| /* | |
| if (1) | |
| { | |
| //find broadphase that contain this multiProxy | |
| int numChildBroadphases = getBroadphaseArray().size(); | |
| for (int i=0;i<numChildBroadphases;i++) | |
| { | |
| btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; | |
| btVector3 worldAabbMin,worldAabbMax; | |
| childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); | |
| bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); | |
| // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); | |
| int containingBroadphaseIndex = -1; | |
| //if already contains this | |
| for (int i=0;i<multiProxy->m_bridgeProxies.size();i++) | |
| { | |
| if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) | |
| { | |
| containingBroadphaseIndex = i; | |
| } | |
| alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); | |
| } | |
| if (overlapsBroadphase) | |
| { | |
| if (containingBroadphaseIndex<0) | |
| { | |
| btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); | |
| childProxy->m_multiSapParentProxy = multiProxy; | |
| addToChildBroadphase(multiProxy,childProxy,childBroadphase); | |
| } | |
| } else | |
| { | |
| if (containingBroadphaseIndex>=0) | |
| { | |
| //remove | |
| btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; | |
| btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; | |
| bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); | |
| multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); | |
| multiProxy->m_bridgeProxies.pop_back(); | |
| } | |
| } | |
| } | |
| ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) | |
| ///hopefully we don't end up with many entries here (can assert/provide feedback on stats) | |
| if (0)//!multiProxy->m_bridgeProxies.size()) | |
| { | |
| ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision | |
| ///this is needed to be able to calculate the aabb overlap | |
| btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); | |
| childProxy->m_multiSapParentProxy = multiProxy; | |
| addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); | |
| } | |
| } | |
| if (!multiProxy->m_bridgeProxies.size()) | |
| { | |
| ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision | |
| ///this is needed to be able to calculate the aabb overlap | |
| btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); | |
| childProxy->m_multiSapParentProxy = multiProxy; | |
| addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); | |
| } | |
| */ | |
| //update | |
| for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) | |
| { | |
| btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; | |
| bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); | |
| } | |
| } | |
| bool stopUpdating=false; | |
| class btMultiSapBroadphasePairSortPredicate | |
| { | |
| public: | |
| bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) | |
| { | |
| btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; | |
| btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; | |
| btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0; | |
| btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0; | |
| return aProxy0 > bProxy0 || | |
| (aProxy0 == bProxy0 && aProxy1 > bProxy1) || | |
| (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); | |
| } | |
| }; | |
| ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb | |
| void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) | |
| { | |
| // m_simpleBroadphase->calculateOverlappingPairs(dispatcher); | |
| if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) | |
| { | |
| btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); | |
| // quicksort(overlappingPairArray,0,overlappingPairArray.size()); | |
| overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); | |
| //perform a sort, to find duplicates and to sort 'invalid' pairs to the end | |
| // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); | |
| overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); | |
| m_invalidPair = 0; | |
| int i; | |
| btBroadphasePair previousPair; | |
| previousPair.m_pProxy0 = 0; | |
| previousPair.m_pProxy1 = 0; | |
| previousPair.m_algorithm = 0; | |
| for (i=0;i<overlappingPairArray.size();i++) | |
| { | |
| btBroadphasePair& pair = overlappingPairArray[i]; | |
| btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0; | |
| btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0; | |
| btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0; | |
| btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; | |
| bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); | |
| previousPair = pair; | |
| bool needsRemoval = false; | |
| if (!isDuplicate) | |
| { | |
| bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); | |
| if (hasOverlap) | |
| { | |
| needsRemoval = false;//callback->processOverlap(pair); | |
| } else | |
| { | |
| needsRemoval = true; | |
| } | |
| } else | |
| { | |
| //remove duplicate | |
| needsRemoval = true; | |
| //should have no algorithm | |
| btAssert(!pair.m_algorithm); | |
| } | |
| if (needsRemoval) | |
| { | |
| getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); | |
| // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); | |
| // m_overlappingPairArray.pop_back(); | |
| pair.m_pProxy0 = 0; | |
| pair.m_pProxy1 = 0; | |
| m_invalidPair++; | |
| gOverlappingPairs--; | |
| } | |
| } | |
| ///if you don't like to skip the invalid pairs in the array, execute following code: | |
| #define CLEAN_INVALID_PAIRS 1 | |
| #ifdef CLEAN_INVALID_PAIRS | |
| //perform a sort, to sort 'invalid' pairs to the end | |
| //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); | |
| overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); | |
| overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); | |
| m_invalidPair = 0; | |
| #endif//CLEAN_INVALID_PAIRS | |
| //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); | |
| } | |
| } | |
| bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) | |
| { | |
| btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; | |
| btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; | |
| return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, | |
| multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); | |
| } | |
| void btMultiSapBroadphase::printStats() | |
| { | |
| /* printf("---------------------------------\n"); | |
| printf("btMultiSapBroadphase.h\n"); | |
| printf("numHandles = %d\n",m_multiSapProxies.size()); | |
| //find broadphase that contain this multiProxy | |
| int numChildBroadphases = getBroadphaseArray().size(); | |
| for (int i=0;i<numChildBroadphases;i++) | |
| { | |
| btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; | |
| childBroadphase->printStats(); | |
| } | |
| */ | |
| } | |
| void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) | |
| { | |
| // not yet | |
| } |