From e792f0e54970d0222f524e34c08b58968b68a362 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Thu, 12 Sep 2013 19:28:39 +0000 Subject: [PATCH] Add faster processing of btCompoundShape versus btCompoundShape, using a dedicated btCompoundCompoundCollisionAlgorithm, with hash pair caching and tandem tree traversal. It should be much faster, when using many child shapes for both compounds. Fix iOS compilation, added header. --- .../btOverlappingPairCache.cpp | 2 +- .../btOverlappingPairCache.h | 11 +- src/BulletCollision/CMakeLists.txt | 4 + .../btCompoundCompoundCollisionAlgorithm.cpp | 421 ++++++++++++++++++ .../btCompoundCompoundCollisionAlgorithm.h | 90 ++++ .../btDefaultCollisionConfiguration.cpp | 15 + .../btDefaultCollisionConfiguration.h | 2 + .../btHashedSimplePairCache.cpp | 278 ++++++++++++ .../btHashedSimplePairCache.h | 174 ++++++++ src/LinearMath/btVector3.cpp | 39 +- src/Makefile.am | 8 +- 11 files changed, 1018 insertions(+), 26 deletions(-) create mode 100644 src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp create mode 100644 src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h create mode 100644 src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp create mode 100644 src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 041bbe05a..ae22dadc7 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -53,7 +53,7 @@ btHashedOverlappingPairCache::~btHashedOverlappingPairCache() void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) { - if (pair.m_algorithm) + if (pair.m_algorithm && dispatcher) { { pair.m_algorithm->~btCollisionAlgorithm(); diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 7a3806c1d..eee90e473 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -96,6 +96,12 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache btOverlapFilterCallback* m_overlapFilterCallback; bool m_blockedForChanges; +protected: + + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; + btOverlappingPairCallback* m_ghostPairCallback; + public: btHashedOverlappingPairCache(); @@ -265,11 +271,6 @@ private: virtual void sortOverlappingPairs(btDispatcher* dispatcher); -protected: - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - btOverlappingPairCallback* m_ghostPairCallback; }; diff --git a/src/BulletCollision/CMakeLists.txt b/src/BulletCollision/CMakeLists.txt index 693853461..62bd7e426 100644 --- a/src/BulletCollision/CMakeLists.txt +++ b/src/BulletCollision/CMakeLists.txt @@ -19,6 +19,7 @@ SET(BulletCollision_SRCS CollisionDispatch/btCollisionObject.cpp CollisionDispatch/btCollisionWorld.cpp CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp CollisionDispatch/btConvexConvexAlgorithm.cpp CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -26,6 +27,7 @@ SET(BulletCollision_SRCS CollisionDispatch/btDefaultCollisionConfiguration.cpp CollisionDispatch/btEmptyCollisionAlgorithm.cpp CollisionDispatch/btGhostObject.cpp + CollisionDispatch/btHashedSimplePairCache.cpp CollisionDispatch/btInternalEdgeUtility.cpp CollisionDispatch/btInternalEdgeUtility.h CollisionDispatch/btManifoldResult.cpp @@ -125,6 +127,7 @@ SET(CollisionDispatch_HDRS CollisionDispatch/btCollisionObjectWrapper.h CollisionDispatch/btCollisionWorld.h CollisionDispatch/btCompoundCollisionAlgorithm.h + CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h CollisionDispatch/btConvexConcaveCollisionAlgorithm.h CollisionDispatch/btConvexConvexAlgorithm.h CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -132,6 +135,7 @@ SET(CollisionDispatch_HDRS CollisionDispatch/btDefaultCollisionConfiguration.h CollisionDispatch/btEmptyCollisionAlgorithm.h CollisionDispatch/btGhostObject.h + CollisionDispatch/btHashedSimplePairCache.h CollisionDispatch/btManifoldResult.h CollisionDispatch/btSimulationIslandManager.h CollisionDispatch/btSphereBoxCollisionAlgorithm.h diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp new file mode 100644 index 000000000..3f5c08e61 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -0,0 +1,421 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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 "btCompoundCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAabbUtil2.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + + +btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; + +btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) +:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_sharedManifold(ci.m_manifold) +{ + m_ownsManifold = false; + + void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); + m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + + const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + + const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); + m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); + + const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); + m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); + + +} + + +btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() +{ + removeChildAlgorithms(); + m_childCollisionAlgorithmCache->~btHashedSimplePairCache(); + btAlignedFree(m_childCollisionAlgorithmCache); +} + +void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +{ + int i; + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;igetAllContactManifolds(manifoldArray); + } + } +} + + +void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() +{ + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int numChildren = pairs.size(); + int i; + for (i=0;i~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } + } + m_childCollisionAlgorithmCache->removeAllPairs(); +} + +struct btCompoundCompoundLeafCallback : btDbvt::ICollide +{ + int m_numOverlapPairs; + + + const btCollisionObjectWrapper* m_compound0ColObjWrap; + const btCollisionObjectWrapper* m_compound1ColObjWrap; + btDispatcher* m_dispatcher; + const btDispatcherInfo& m_dispatchInfo; + btManifoldResult* m_resultOut; + + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + + btPersistentManifold* m_sharedManifold; + + btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, + const btCollisionObjectWrapper* compound0ObjWrap, + btDispatcher* dispatcher, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut, + btHashedSimplePairCache* childAlgorithmsCache, + btPersistentManifold* sharedManifold) + :m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), + m_childCollisionAlgorithmCache(childAlgorithmsCache), + m_sharedManifold(sharedManifold), + m_numOverlapPairs(0) + { + + } + + + + + void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) + { + m_numOverlapPairs++; + + + int childIndex0 = leaf0->dataAsInt; + int childIndex1 = leaf1->dataAsInt; + + + btAssert(childIndex0>=0); + btAssert(childIndex1>=0); + + + const btCompoundShape* compoundShape0 = static_cast(m_compound0ColObjWrap->getCollisionShape()); + btAssert(childIndex0getNumChildShapes()); + + const btCompoundShape* compoundShape1 = static_cast(m_compound1ColObjWrap->getCollisionShape()); + btAssert(childIndex1getNumChildShapes()); + + const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); + const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); + + //backup + btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); + btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; + + btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); + btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; + + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + + if (gCompoundCompoundChildShapePairCallback) + { + if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) + return; + } + + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); + btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); + + + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); + + btCollisionAlgorithm* colAlgo = 0; + + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; + + } else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } + + btAssert(colAlgo); + + const btCollisionObjectWrapper* tmpWrap0 = 0; + const btCollisionObjectWrapper* tmpWrap1 = 0; + + tmpWrap0 = m_resultOut->getBody0Wrap(); + tmpWrap1 = m_resultOut->getBody1Wrap(); + + m_resultOut->setBody0Wrap(&compoundWrap0); + m_resultOut->setBody1Wrap(&compoundWrap1); + + m_resultOut->setShapeIdentifiersA(-1,childIndex0); + m_resultOut->setShapeIdentifiersB(-1,childIndex1); + + + colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); + + m_resultOut->setBody0Wrap(tmpWrap0); + m_resultOut->setBody1Wrap(tmpWrap1); + + + + } + } +}; + + +static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, const btTransform& xform) +{ + btVector3 newmin,newmax; + btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); + btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); + return Intersect(a,newb); +} + + +static inline void MycollideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback) +{ + + if(root0&&root1) + { + int depth=1; + int treshold=btDbvt::DOUBLE_STACKSIZE-4; + btAlignedObjectArray stkStack; + stkStack.resize(btDbvt::DOUBLE_STACKSIZE); + stkStack[0]=btDbvt::sStkNN(root0,root1); + do { + btDbvt::sStkNN p=stkStack[--depth]; + if(MyIntersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); + } + else + { + callback->Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; + + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); + const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); + + ///btCompoundShape might have changed: + ////make sure the internal child collision algorithm caches are still valid + if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) + { + ///clear all + removeChildAlgorithms(); + } + + + ///we need to refresh all contact manifolds + ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep + ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm + { + int i; + btManifoldArray manifoldArray; + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;igetAllContactManifolds(manifoldArray); + for (int m=0;mgetNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0); + } + } + manifoldArray.resize(0); + } + } + } + + + const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); + const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); + + btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); + + + const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); + MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); + + //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); + + //remove non-overlapping child pairs + + { + btAssert(m_removePairs.size()==0); + + //iterate over all children, perform an AABB check inside ProcessChildShape + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int i; + btManifoldArray manifoldArray; + + + + + + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + + for (i=0;igetChildShape(pairs[i].m_indexA); + orgTrans0 = col0ObjWrap->getWorldTransform(); + orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); + newChildWorldTrans0 = orgTrans0*childTrans0 ; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + } + + { + btTransform orgInterpolationTrans1; + const btCollisionShape* childShape1 = 0; + btTransform orgTrans1; + btTransform newChildWorldTrans1; + + childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); + orgTrans1 = col1ObjWrap->getWorldTransform(); + orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); + newChildWorldTrans1 = orgTrans1*childTrans1 ; + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + } + + + + if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); + } + } + } + for (int i=0;iremoveOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); + } + m_removePairs.clear(); + } + +} + +btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btAssert(0); + return 0.f; + +} + + + diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h new file mode 100644 index 000000000..8aba3919d --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -0,0 +1,90 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. + +*/ + +#ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h" +class btDispatcher; +class btCollisionObject; + +class btCollisionShape; +typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); +extern btShapePairCallback gCompoundCompoundChildShapePairCallback; + +/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes +class btCompoundCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + btSimplePairArray m_removePairs; + + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; + + + int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated + int m_compoundShapeRevision1; + + void removeChildAlgorithms(); + +// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + +public: + + btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + virtual ~btCompoundCompoundCollisionAlgorithm(); + + + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + } + }; + +}; + +#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 6529497ba..c3cacec4a 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -19,6 +19,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h" + #include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" @@ -64,6 +66,10 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16); + m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); @@ -155,6 +161,9 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_compoundCreateFunc); + m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree(m_compoundCompoundCreateFunc); + m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_swappedCompoundCreateFunc); @@ -258,6 +267,12 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_swappedConvexConcaveCreateFunc; } + + if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) + { + return m_compoundCompoundCreateFunc; + } + if (btBroadphaseProxy::isCompound(proxyType0)) { return m_compoundCreateFunc; diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index bfec6d361..2078420e1 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -69,6 +69,8 @@ protected: btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; btCollisionAlgorithmCreateFunc* m_sphereSphereCF; diff --git a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp new file mode 100644 index 000000000..933400bb1 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -0,0 +1,278 @@ +/* +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 "btHashedSimplePairCache.h" + + +#include + +int gOverlappingSimplePairs = 0; +int gRemoveSimplePairs =0; +int gAddedSimplePairs =0; +int gFindSimplePairs =0; + + + + +btHashedSimplePairCache::btHashedSimplePairCache(): + m_blockedForChanges(false) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +btHashedSimplePairCache::~btHashedSimplePairCache() +{ +} + + + + + + +void btHashedSimplePairCache::removeAllPairs() +{ + m_overlappingPairArray.clear(); + m_hashTable.clear(); + m_next.clear(); + + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + +btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) +{ + gFindSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false) + { + index = m_next[index]; + } + + if (index == BT_SIMPLE_NULL_PAIR) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include + +void btHashedSimplePairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_SIMPLE_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_SIMPLE_NULL_PAIR; + } + + for(i=0;i(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) +{ + + int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair != NULL) + { + return pair; + } + + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); + + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + } + + pair = new (mem) btSimplePair(indexA,indexB); + + pair->m_userPointer = 0; + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) +{ + gRemoveSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair == NULL) + { + return 0; + } + + + void* userData = pair->m_userPointer; + + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + btAssert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + int previous = BT_SIMPLE_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_overlappingPairArray.pop_back(); + return userData; + } + + // Remove the last pair from the hash table. + const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast(getHash(static_cast(last->m_indexA), static_cast(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + previous = BT_SIMPLE_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return userData; +} +//#include + + + + + + + + + + diff --git a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h new file mode 100644 index 000000000..ba573743d --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -0,0 +1,174 @@ +/* +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. +*/ + +#ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H +#define BT_HASHED_SIMPLE_PAIR_CACHE_H + + + +#include "LinearMath/btAlignedObjectArray.h" + +const int BT_SIMPLE_NULL_PAIR=0xffffffff; + +struct btSimplePair +{ + btSimplePair(int indexA,int indexB) + :m_indexA(indexA), + m_indexB(indexB), + m_userPointer(0) + { + } + + int m_indexA; + int m_indexB; + union + { + void* m_userPointer; + int m_userValue; + }; +}; + +typedef btAlignedObjectArray btSimplePairArray; + + + +extern int gOverlappingSimplePairs; +extern int gRemoveSimplePairs; +extern int gAddedSimplePairs; +extern int gFindSimplePairs; + + + + +class btHashedSimplePairCache +{ + btSimplePairArray m_overlappingPairArray; + + bool m_blockedForChanges; + + +protected: + + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; + + +public: + btHashedSimplePairCache(); + virtual ~btHashedSimplePairCache(); + + void removeAllPairs(); + + virtual void* removeOverlappingPair(int indexA,int indexB); + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual btSimplePair* addOverlappingPair(int indexA,int indexB) + { + gAddedSimplePairs++; + + return internalAddPair(indexA,indexB); + } + + + virtual btSimplePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btSimplePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + btSimplePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btSimplePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + btSimplePair* findPair(int indexA,int indexB); + + int GetCount() const { return m_overlappingPairArray.size(); } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + btSimplePair* internalAddPair(int indexA, int indexB); + + void growTables(); + + SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) + { + return pair.m_indexA == indexA && pair.m_indexB == indexB; + } + + + + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + { + int key = static_cast(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast(key); + } + + + + + + SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) + { + + int index = m_hashTable[hash]; + + while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) + { + index = m_next[index]; + } + + if ( index == BT_SIMPLE_NULL_PAIR ) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + +}; + + + + +#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H + + diff --git a/src/LinearMath/btVector3.cpp b/src/LinearMath/btVector3.cpp index 0af112c75..cf27d61dc 100644 --- a/src/LinearMath/btVector3.cpp +++ b/src/LinearMath/btVector3.cpp @@ -823,7 +823,8 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa #elif defined BT_USE_NEON #define ARM_NEON_GCC_COMPATIBILITY 1 #include - +#include +#include //for sysctlbyname static long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ); static long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ); @@ -836,24 +837,24 @@ long (*_maxdot_large)( const float *vv, const float *vec, unsigned long count, f long (*_mindot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _mindot_large_sel; -static inline uint32_t btGetCpuCapabilities( void ) -{ - static uint32_t capabilities = 0; - static bool testedCapabilities = false; - - if( 0 == testedCapabilities) - { - uint32_t hasFeature = 0; - size_t featureSize = sizeof( hasFeature ); - int err = sysctlbyname( "hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0 ); - - if( 0 == err && hasFeature) - capabilities |= 0x2000; - - testedCapabilities = true; - } - - return capabilities; +static inline uint32_t btGetCpuCapabilities( void ) +{ + static uint32_t capabilities = 0; + static bool testedCapabilities = false; + + if( 0 == testedCapabilities) + { + uint32_t hasFeature = 0; + size_t featureSize = sizeof( hasFeature ); + int err = sysctlbyname( "hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0 ); + + if( 0 == err && hasFeature) + capabilities |= 0x2000; + + testedCapabilities = true; + } + + return capabilities; } diff --git a/src/Makefile.am b/src/Makefile.am index ec39e7a05..a5d987a25 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -160,6 +160,8 @@ libBulletCollision_la_SOURCES = \ BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp \ BulletCollision/CollisionDispatch/btUnionFind.cpp \ BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp \ + BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp \ + BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp \ BulletCollision/CollisionShapes/btTetrahedronShape.cpp \ BulletCollision/CollisionShapes/btShapeHull.cpp \ BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp \ @@ -223,7 +225,7 @@ libBulletCollision_la_SOURCES = \ BulletCollision/NarrowPhaseCollision/btManifoldPoint.h \ BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h \ BulletCollision/CollisionDispatch/btCollisionObject.h \ - BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h \ + BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h \ BulletCollision/CollisionDispatch/btGhostObject.h \ BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h \ BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h \ @@ -239,6 +241,8 @@ libBulletCollision_la_SOURCES = \ BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h \ BulletCollision/CollisionDispatch/btUnionFind.h \ BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h \ + BulletCollision/CollisionDispatch/btHashedSimplePairCache.h \ + BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h \ BulletCollision/CollisionDispatch/btSimulationIslandManager.h \ BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h \ BulletCollision/CollisionDispatch/btCollisionWorld.h \ @@ -501,6 +505,8 @@ nobase_bullet_include_HEADERS += \ BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h \ BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h \ BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h \ + BulletCollision/CollisionDispatch/btHashedSimplePairCache.h \ + BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h \ BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h \ BulletCollision/CollisionDispatch/btGhostObject.h \ BulletCollision/CollisionDispatch/btSimulationIslandManager.h \