diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2007-06-23 09:28:07 +0400 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2007-06-23 09:28:07 +0400 |
commit | ca26aeb7b23e37e65f49d907ea53fcaeee77ad4e (patch) | |
tree | b6530195a8ef5d874b9fc2bbb9574f1484781be5 /extern/bullet2/src/BulletCollision/BroadphaseCollision | |
parent | 14ad8c9941ac1e6f5252a843c6ad17653cbbd689 (diff) |
upgrade to latest Bullet 2.53. cross the fingers it doesn't break one of the exotic or less exotic platforms
Diffstat (limited to 'extern/bullet2/src/BulletCollision/BroadphaseCollision')
10 files changed, 384 insertions, 156 deletions
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index b05285ca727..be4a11506df 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -21,9 +21,34 @@ #include <assert.h> +#ifdef DEBUG_BROADPHASE +#include <stdio.h> +void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality) +{ + int numEdges = m_pHandles[0].m_maxEdges[axis]; + printf("SAP Axis %d, numEdges=%d\n",axis,numEdges); + + int i; + for (i=0;i<numEdges+1;i++) + { + Edge* pEdge = m_pEdges[axis] + i; + Handle* pHandlePrev = getHandle(pEdge->m_handle); + int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; + char beginOrEnd; + beginOrEnd=pEdge->IsMax()?'E':'B'; + printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex); + } + + if (checkCardinality) + assert(numEdges == m_numHandles*2+1); +} +#endif //DEBUG_BROADPHASE + + btBroadphaseProxy* btAxisSweep3::createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask) { - unsigned short handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask); + (void)shapeType; + BP_FP_INT_TYPE handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask); Handle* handle = getHandle(handleId); @@ -40,6 +65,7 @@ void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con { Handle* handle = static_cast<Handle*>(proxy); updateHandle(handle->m_handleId,aabbMin,aabbMax); + } @@ -50,10 +76,11 @@ void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles) :btOverlappingPairCache() { + m_invalidPair = 0; //assert(bounds.HasVolume()); // 1 handle is reserved as sentinel - assert(maxHandles > 1 && maxHandles < 32767); + btAssert(maxHandles > 1 && maxHandles < BP_MAX_HANDLES); // init bounds m_worldAabbMin = worldAabbMin; @@ -61,7 +88,9 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; - m_quantize = btVector3(65535.0f,65535.0f,65535.0f) / aabbSize; + BP_FP_INT_TYPE maxInt = BP_HANDLE_SENTINEL; + + m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; // allocate handles buffer and put all handles on free list m_pHandles = new Handle[maxHandles]; @@ -71,7 +100,7 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab // handle 0 is reserved as the null index, and is also used as the sentinel m_firstFreeHandle = 1; { - for (int i = m_firstFreeHandle; i < maxHandles; i++) + for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) m_pHandles[i].SetNextFree(i + 1); m_pHandles[maxHandles - 1].SetNextFree(0); } @@ -94,9 +123,14 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab m_pEdges[axis][0].m_pos = 0; m_pEdges[axis][0].m_handle = 0; - m_pEdges[axis][1].m_pos = 0xffff; + m_pEdges[axis][1].m_pos = BP_HANDLE_SENTINEL; m_pEdges[axis][1].m_handle = 0; +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } + } btAxisSweep3::~btAxisSweep3() @@ -107,43 +141,36 @@ btAxisSweep3::~btAxisSweep3() delete[] m_pHandles; } -void btAxisSweep3::quantize(unsigned short* out, const btPoint3& point, int isMax) const +void btAxisSweep3::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const { btPoint3 clampedPoint(point); - /* - if (isMax) - clampedPoint += btVector3(10,10,10); - else - { - clampedPoint -= btVector3(10,10,10); - } - */ + clampedPoint.setMax(m_worldAabbMin); clampedPoint.setMin(m_worldAabbMax); btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; - out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax); - out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax); - out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax); + out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & BP_HANDLE_MASK) | isMax); + out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & BP_HANDLE_MASK) | isMax); + out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & BP_HANDLE_MASK) | isMax); } -unsigned short btAxisSweep3::allocHandle() +BP_FP_INT_TYPE btAxisSweep3::allocHandle() { assert(m_firstFreeHandle); - unsigned short handle = m_firstFreeHandle; + BP_FP_INT_TYPE handle = m_firstFreeHandle; m_firstFreeHandle = getHandle(handle)->GetNextFree(); m_numHandles++; return handle; } -void btAxisSweep3::freeHandle(unsigned short handle) +void btAxisSweep3::freeHandle(BP_FP_INT_TYPE handle) { assert(handle > 0 && handle < m_maxHandles); @@ -155,15 +182,15 @@ void btAxisSweep3::freeHandle(unsigned short handle) -unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask) +BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask) { // quantize the bounds - unsigned short min[3], max[3]; + BP_FP_INT_TYPE min[3], max[3]; quantize(min, aabbMin, 0); quantize(max, aabbMax, 1); // allocate a handle - unsigned short handle = allocHandle(); + BP_FP_INT_TYPE handle = allocHandle(); assert(handle!= 0xcdcd); Handle* pHandle = getHandle(handle); @@ -175,11 +202,13 @@ unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a pHandle->m_collisionFilterMask = collisionFilterMask; // compute current limit of edge arrays - int limit = m_numHandles * 2; + BP_FP_INT_TYPE limit = m_numHandles * 2; + // insert new edges just inside the max boundary edge - for (int axis = 0; axis < 3; axis++) + for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++) { + m_pHandles[0].m_maxEdges[axis] += 2; m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; @@ -202,14 +231,14 @@ unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a sortMinDown(2, pHandle->m_minEdges[2], true); sortMaxDown(2, pHandle->m_maxEdges[2], true); - //PrintAxis(1); return handle; } -void btAxisSweep3::removeHandle(unsigned short handle) +void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle) { + Handle* pHandle = getHandle(handle); //explicitly remove the pairs containing the proxy @@ -220,42 +249,145 @@ void btAxisSweep3::removeHandle(unsigned short handle) // compute current limit of edge arrays int limit = m_numHandles * 2; + int axis; for (axis = 0;axis<3;axis++) { - Edge* pEdges = m_pEdges[axis]; - int maxEdge= pHandle->m_maxEdges[axis]; - pEdges[maxEdge].m_pos = 0xffff; - int minEdge = pHandle->m_minEdges[axis]; - pEdges[minEdge].m_pos = 0xffff; + m_pHandles[0].m_maxEdges[axis] -= 2; } // remove the edges by sorting them up to the end of the list for ( axis = 0; axis < 3; axis++) { Edge* pEdges = m_pEdges[axis]; - int max = pHandle->m_maxEdges[axis]; - pEdges[max].m_pos = 0xffff; + BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; + pEdges[max].m_pos = BP_HANDLE_SENTINEL; sortMaxUp(axis,max,false); - - int i = pHandle->m_minEdges[axis]; - pEdges[i].m_pos = 0xffff; + + + BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; + pEdges[i].m_pos = BP_HANDLE_SENTINEL; + sortMinUp(axis,i,false); pEdges[limit-1].m_handle = 0; - pEdges[limit-1].m_pos = 0xffff; + pEdges[limit-1].m_pos = BP_HANDLE_SENTINEL; + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis,false); +#endif //DEBUG_BROADPHASE + } + // free the handle freeHandle(handle); } +extern int gOverlappingPairs; + + +void btAxisSweep3::refreshOverlappingPairs() +{ + +} +void btAxisSweep3::processAllOverlappingPairs(btOverlapCallback* callback) +{ + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + m_overlappingPairArray.heapSort(btBroadphasePairSortPredicate()); + + //remove the 'invalid' ones +#ifdef USE_POPBACK_REMOVAL + while (m_invalidPair>0) + { + m_invalidPair--; + m_overlappingPairArray.pop_back(); + } +#else + m_overlappingPairArray.resize(m_overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; +#endif + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;i<m_overlappingPairArray.size();i++) + { + + btBroadphasePair& pair = m_overlappingPairArray[i]; + + bool isDuplicate = (pair == previousPair); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + bool hasOverlap = testOverlap(pair.m_pProxy0,pair.m_pProxy1); + + if (hasOverlap) + { + needsRemoval = callback->processOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + cleanOverlappingPair(pair); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + m_invalidPair++; + gOverlappingPairs--; + } + + } +} + + +bool btAxisSweep3::testOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + const Handle* pHandleA = static_cast<Handle*>(proxy0); + const Handle* pHandleB = static_cast<Handle*>(proxy1); + + //optimization 1: check the array index (memory address), instead of the m_pos + + for (int axis = 0; axis < 3; axis++) + { + if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || + pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) + { + return false; + } + } + return true; +} + bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB) { //optimization 1: check the array index (memory address), instead of the m_pos @@ -272,7 +404,7 @@ bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Hand } } - //optimization 2: only 2 axis need to be tested + //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization) /*for (int axis = 0; axis < 3; axis++) { @@ -287,7 +419,7 @@ bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Hand return true; } -void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax) +void btAxisSweep3::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax) { // assert(bounds.IsFinite()); //assert(bounds.HasVolume()); @@ -295,15 +427,15 @@ void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,c Handle* pHandle = getHandle(handle); // quantize the new bounds - unsigned short min[3], max[3]; + BP_FP_INT_TYPE min[3], max[3]; quantize(min, aabbMin, 0); quantize(max, aabbMax, 1); // update changed edges for (int axis = 0; axis < 3; axis++) { - unsigned short emin = pHandle->m_minEdges[axis]; - unsigned short emax = pHandle->m_maxEdges[axis]; + BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis]; + BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis]; int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos; int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos; @@ -324,14 +456,22 @@ void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,c if (dmax < 0) sortMaxDown(axis, emax); + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE } - //PrintAxis(1); + } + + + // sorting a min edge downwards can only ever *add* overlaps -void btAxisSweep3::sortMinDown(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { + Edge* pEdge = m_pEdges[axis] + edge; Edge* pPrev = pEdge - 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); @@ -368,16 +508,21 @@ void btAxisSweep3::sortMinDown(int axis, unsigned short edge, bool updateOverlap pEdge--; pPrev--; } + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } // sorting a min edge upwards can only ever *remove* overlaps -void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { Edge* pEdge = m_pEdges[axis] + edge; Edge* pNext = pEdge + 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); - while (pEdge->m_pos > pNext->m_pos) + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) { Handle* pHandleNext = getHandle(pNext->m_handle); @@ -386,10 +531,12 @@ void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps) // if next edge is maximum remove any overlap between the two handles if (updateOverlaps) { + /* Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pNext->m_handle); btBroadphasePair tmpPair(*handle0,*handle1); removeOverlappingPair(tmpPair); + */ } @@ -410,11 +557,14 @@ void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps) pEdge++; pNext++; } + + } // sorting a max edge downwards can only ever *remove* overlaps -void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { + Edge* pEdge = m_pEdges[axis] + edge; Edge* pPrev = pEdge - 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); @@ -428,6 +578,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap // if previous edge was a minimum remove any overlap between the two handles if (updateOverlaps) { + //this is done during the overlappingpairarray iteration/narrowphase collision + /* Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pPrev->m_handle); btBroadphasePair* pair = findPair(handle0,handle1); @@ -437,6 +589,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap { removeOverlappingPair(*pair); } + */ + } // update edge reference in other handle @@ -456,16 +610,22 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap pEdge--; pPrev--; } + + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } // sorting a max edge upwards can only ever *add* overlaps -void btAxisSweep3::sortMaxUp(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { Edge* pEdge = m_pEdges[axis] + edge; Edge* pNext = pEdge + 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); - while (pEdge->m_pos > pNext->m_pos) + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) { Handle* pHandleNext = getHandle(pNext->m_handle); @@ -496,4 +656,5 @@ void btAxisSweep3::sortMaxUp(int axis, unsigned short edge, bool updateOverlaps) pEdge++; pNext++; } + } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index ebbbe01bbe6..57bbb368672 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -19,11 +19,29 @@ #ifndef AXIS_SWEEP_3_H #define AXIS_SWEEP_3_H -#include "LinearMath/btPoint3.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btVector3.h" #include "btOverlappingPairCache.h" #include "btBroadphaseProxy.h" + +//Enable BP_USE_FIXEDPOINT_INT_32 if you need more then 32767 objects +//#define BP_USE_FIXEDPOINT_INT_32 1 + +#ifdef BP_USE_FIXEDPOINT_INT_32 + #define BP_FP_INT_TYPE unsigned int + #define BP_MAX_HANDLES 1500000 //arbitrary maximum number of handles + #define BP_HANDLE_SENTINEL 0x7fffffff + #define BP_HANDLE_MASK 0xfffffffe +#else + #define BP_FP_INT_TYPE unsigned short int + #define BP_MAX_HANDLES 32767 + #define BP_HANDLE_SENTINEL 0xffff + #define BP_HANDLE_MASK 0xfffe +#endif //BP_USE_FIXEDPOINT_INT_32 + +//#define DEBUG_BROADPHASE 1 + /// btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. /// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats. /// The testOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos @@ -36,10 +54,10 @@ public: class Edge { public: - unsigned short m_pos; // low bit is min/max - unsigned short m_handle; + BP_FP_INT_TYPE m_pos; // low bit is min/max + BP_FP_INT_TYPE m_handle; - unsigned short IsMax() const {return m_pos & 1;} + BP_FP_INT_TYPE IsMax() const {return m_pos & 1;} }; public: @@ -48,14 +66,14 @@ public: public: // indexes into the edge arrays - unsigned short m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 - unsigned short m_handleId; - unsigned short m_pad; + BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 + BP_FP_INT_TYPE m_handleId; + BP_FP_INT_TYPE m_pad; //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject - inline void SetNextFree(unsigned short next) {m_minEdges[0] = next;} - inline unsigned short GetNextFree() const {return m_minEdges[0];} + inline void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} + inline BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry @@ -65,51 +83,56 @@ private: btVector3 m_quantize; // scaling factor for quantization - int m_numHandles; // number of active handles + BP_FP_INT_TYPE m_numHandles; // number of active handles int m_maxHandles; // max number of handles Handle* m_pHandles; // handles pool - unsigned short m_firstFreeHandle; // free handles list + BP_FP_INT_TYPE m_firstFreeHandle; // free handles list Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) + int m_invalidPair; // allocation/deallocation - unsigned short allocHandle(); - void freeHandle(unsigned short handle); + BP_FP_INT_TYPE allocHandle(); + void freeHandle(BP_FP_INT_TYPE handle); bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB); - //Overlap* AddOverlap(unsigned short handleA, unsigned short handleB); - //void RemoveOverlap(unsigned short handleA, unsigned short handleB); +#ifdef DEBUG_BROADPHASE + void debugPrintAxis(int axis,bool checkCardinality=true); +#endif //DEBUG_BROADPHASE - void quantize(unsigned short* out, const btPoint3& point, int isMax) const; + //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); + //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - void sortMinDown(int axis, unsigned short edge, bool updateOverlaps = true); - void sortMinUp(int axis, unsigned short edge, bool updateOverlaps = true); - void sortMaxDown(int axis, unsigned short edge, bool updateOverlaps = true); - void sortMaxUp(int axis, unsigned short edge, bool updateOverlaps = true); + void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const; + + void sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); + void sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); + void sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); + void sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); public: btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384); virtual ~btAxisSweep3(); - virtual void refreshOverlappingPairs() - { - //this is replace by sweep and prune - } + virtual void refreshOverlappingPairs(); - unsigned short addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask); - void removeHandle(unsigned short handle); - void updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax); - inline Handle* getHandle(unsigned short index) const {return m_pHandles + index;} + BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask); + void removeHandle(BP_FP_INT_TYPE handle); + void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax); + inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} + void processAllOverlappingPairs(btOverlapCallback* callback); //Broadphase Interface virtual btBroadphaseProxy* createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); virtual void destroyProxy(btBroadphaseProxy* proxy); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); + bool testOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); }; -#endif //AXIS_SWEEP_3_H +#endif + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index 0c0bfe4f7b9..b6ace03c07a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -21,7 +21,7 @@ subject to the following restrictions: struct btDispatcherInfo; class btDispatcher; struct btBroadphaseProxy; -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" ///BroadphaseInterface for aabb-overlapping object pairs class btBroadphaseInterface diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index b279022c802..40d9748ffa9 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -16,6 +16,7 @@ subject to the following restrictions: #ifndef BROADPHASE_PROXY_H #define BROADPHASE_PROXY_H +#include "../../LinearMath/btScalar.h" //for SIMD_FORCE_INLINE /// btDispatcher uses these types @@ -33,6 +34,7 @@ enum BroadphaseNativeTypes IMPLICIT_CONVEX_SHAPES_START_HERE, SPHERE_SHAPE_PROXYTYPE, MULTI_SPHERE_SHAPE_PROXYTYPE, + CAPSULE_SHAPE_PROXYTYPE, CONE_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE, CYLINDER_SHAPE_PROXYTYPE, @@ -71,7 +73,7 @@ struct btBroadphaseProxy KinematicFilter = 4, DebrisFilter = 8, SensorTrigger = 16, - AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger, + AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger }; //Usually the client btCollisionObject or Rigidbody class @@ -113,7 +115,8 @@ struct btBroadphaseProxy return (proxyType == STATIC_PLANE_PROXYTYPE); } -}; +} +; class btCollisionAlgorithm; @@ -128,14 +131,16 @@ struct btBroadphasePair : m_pProxy0(0), m_pProxy1(0), - m_algorithm(0) + m_algorithm(0), + m_userInfo(0) { } btBroadphasePair(const btBroadphasePair& other) : m_pProxy0(other.m_pProxy0), m_pProxy1(other.m_pProxy1), - m_algorithm(other.m_algorithm) + m_algorithm(other.m_algorithm), + m_userInfo(other.m_userInfo) { } btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) @@ -154,6 +159,7 @@ struct btBroadphasePair } m_algorithm = 0; + m_userInfo = 0; } @@ -161,15 +167,37 @@ struct btBroadphasePair btBroadphaseProxy* m_pProxy1; mutable btCollisionAlgorithm* m_algorithm; -}; + mutable void* m_userInfo; +}; +/* //comparison for set operation, see Solid DT_Encounter -inline bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) +SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) { return a.m_pProxy0 < b.m_pProxy0 || (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1); } +*/ + + +class btBroadphasePairSortPredicate +{ + public: + + bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) + { + return a.m_pProxy0 > b.m_pProxy0 || + (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) || + (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); + } +}; + + +SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) +{ + return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1); +} #endif //BROADPHASE_PROXY_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index f9e22057058..55cec386a7b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -16,6 +16,8 @@ subject to the following restrictions: #ifndef COLLISION_ALGORITHM_H #define COLLISION_ALGORITHM_H +#include "../../LinearMath/btScalar.h" + struct btBroadphaseProxy; class btDispatcher; class btManifoldResult; @@ -34,6 +36,7 @@ struct btCollisionAlgorithmConstructionInfo btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) :m_dispatcher(dispatcher) { + (void)temp; } btDispatcher* m_dispatcher; @@ -66,7 +69,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index c7714f592c4..3d958cc8fef 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -16,6 +16,8 @@ subject to the following restrictions: #ifndef _DISPATCHER_H #define _DISPATCHER_H +#include "../../LinearMath/btScalar.h" + class btCollisionAlgorithm; struct btBroadphaseProxy; class btRigidBody; @@ -34,10 +36,10 @@ struct btDispatcherInfo DISPATCH_CONTINUOUS }; btDispatcherInfo() - :m_timeStep(0.f), + :m_timeStep(btScalar(0.)), m_stepCount(0), m_dispatchFunc(DISPATCH_DISCRETE), - m_timeOfImpact(1.f), + m_timeOfImpact(btScalar(1.)), m_useContinuous(false), m_debugDraw(0), m_enableSatConvex(false), @@ -46,10 +48,10 @@ struct btDispatcherInfo { } - float m_timeStep; + btScalar m_timeStep; int m_stepCount; int m_dispatchFunc; - float m_timeOfImpact; + btScalar m_timeOfImpact; bool m_useContinuous; class btIDebugDraw* m_debugDraw; bool m_enableSatConvex; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 5e3fa633589..4187a8f2970 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -39,15 +39,15 @@ btOverlappingPairCache::~btOverlappingPairCache() void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair) { - std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.find(findPair); -// assert(it != m_overlappingPairSet.end()); - - if (it != m_overlappingPairSet.end()) + int findIndex = m_overlappingPairArray.findLinearSearch(findPair); + if (findIndex < m_overlappingPairArray.size()) { gOverlappingPairs--; - btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); - cleanOverlappingPair(*pair); - m_overlappingPairSet.erase(it); + btBroadphasePair& pair = m_overlappingPairArray[findIndex]; + cleanOverlappingPair(pair); + + m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); } } @@ -78,7 +78,7 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa btBroadphasePair pair(*proxy0,*proxy1); - m_overlappingPairSet.insert(pair); + m_overlappingPairArray.push_back(pair); gOverlappingPairs++; } @@ -93,13 +93,15 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa return 0; btBroadphasePair tmpPair(*proxy0,*proxy1); - std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.find(tmpPair); - if ((it == m_overlappingPairSet.end())) - return 0; + int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); - //assert(it != m_overlappingPairSet.end()); - btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); - return pair; + if (findIndex < m_overlappingPairArray.size()) + { + //assert(it != m_overlappingPairSet.end()); + btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; + return pair; + } + return 0; } @@ -170,30 +172,23 @@ void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP void btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback) { - std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.begin(); - for (; !(it==m_overlappingPairSet.end());) + + int i; + + for (i=0;i<m_overlappingPairArray.size();) { - btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); + btBroadphasePair* pair = &m_overlappingPairArray[i]; if (callback->processOverlap(*pair)) { cleanOverlappingPair(*pair); - std::set<btBroadphasePair>::iterator it2 = it; - //why does next line not compile under OS X?? -#ifdef MAC_OSX_FIXED_STL_SET - it2++; - it = m_overlappingPairSet.erase(it); - assert(it == it2); -#else - it++; - m_overlappingPairSet.erase(it2); -#endif //MAC_OSX_FIXED_STL_SET - + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); gOverlappingPairs--; } else { - it++; + i++; } } } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index bc62961bf3c..e3442212171 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -20,8 +20,8 @@ subject to the following restrictions: #include "btBroadphaseInterface.h" #include "btBroadphaseProxy.h" -#include "LinearMath/btPoint3.h" -#include <set> +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btAlignedObjectArray.h" struct btOverlapCallback @@ -37,47 +37,61 @@ virtual ~btOverlapCallback() ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase class btOverlappingPairCache : public btBroadphaseInterface { - //avoid brute-force finding all the time - std::set<btBroadphasePair> m_overlappingPairSet; - - //during the dispatch, check that user doesn't destroy/create proxy - bool m_blockedForChanges; - - public: + protected: + //avoid brute-force finding all the time + btAlignedObjectArray<btBroadphasePair> m_overlappingPairArray; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; - btOverlappingPairCache(); - virtual ~btOverlappingPairCache(); + public: + + btOverlappingPairCache(); + virtual ~btOverlappingPairCache(); - void processAllOverlappingPairs(btOverlapCallback*); + virtual void processAllOverlappingPairs(btOverlapCallback*); - void removeOverlappingPair(btBroadphasePair& pair); + void removeOverlappingPair(btBroadphasePair& pair); - void cleanOverlappingPair(btBroadphasePair& pair); - - void addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + void cleanOverlappingPair(btBroadphasePair& pair); + + void addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + - - void cleanProxyFromPairs(btBroadphaseProxy* proxy); + void cleanProxyFromPairs(btBroadphaseProxy* proxy); - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy); + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy); - inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const - { - bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; - collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - - return collides; - } + inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + - - virtual void refreshOverlappingPairs() =0; + virtual void refreshOverlappingPairs() =0; + btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } + }; #endif //OVERLAPPING_PAIR_CACHE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 6281e93eeb4..30bcbe0c5f1 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btMatrix3x3.h" -#include <vector> +#include <new> void btSimpleBroadphase::validate() @@ -85,8 +85,8 @@ btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& min, cons btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; - int index = proxy - proxy1; - assert(index == freeIndex); + int index = int(proxy - proxy1); + btAssert(index == freeIndex); m_pProxies[m_numProxies] = proxy; m_numProxies++; @@ -100,7 +100,8 @@ class RemovingOverlapCallback : public btOverlapCallback protected: virtual bool processOverlap(btBroadphasePair& pair) { - assert(0); + (void)pair; + btAssert(0); return false; } }; @@ -131,8 +132,8 @@ void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg) btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg); btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; - int index = proxy0 - proxy1; - assert (index < m_maxProxies); + int index = int(proxy0 - proxy1); + btAssert (index < m_maxProxies); m_freeProxies[--m_firstFreeProxy] = index; removeOverlappingPairsContainingProxy(proxyOrg); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 281677081d7..1f265f3dd6b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -31,6 +31,7 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), m_min(minpt),m_max(maxpt) { + (void)shapeType; } |