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 | |
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')
164 files changed, 5013 insertions, 2176 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; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 0d76b1013f9..81133670f0c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +#include "LinearMath/btScalar.h" #include "SphereTriangleDetector.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -28,13 +29,14 @@ m_triangle(triangle) void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) { + (void)debugDraw; const btTransform& transformA = input.m_transformA; const btTransform& transformB = input.m_transformB; btVector3 point,normal; - btScalar timeOfImpact = 1.f; - btScalar depth = 0.f; -// output.m_distance = 1e30f; + btScalar timeOfImpact = btScalar(1.); + btScalar depth = btScalar(0.); +// output.m_distance = btScalar(1e30); //move sphere into triangle space btTransform sphereInTr = transformB.inverseTimes(transformA); @@ -45,19 +47,19 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res } -#define MAX_OVERLAP 0.f +#define MAX_OVERLAP btScalar(0.) // See also geometrictools.com // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv -float SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { btVector3 diff = p - from; btVector3 v = to - from; - float t = v.dot(diff); + btScalar t = v.dot(diff); if (t > 0) { - float dotVV = v.dot(v); + btScalar dotVV = v.dot(v); if (t < dotVV) { t /= dotVV; diff -= t*v; @@ -80,7 +82,7 @@ bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* ve } ///combined discrete/continuous sphere-triangle -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact) +bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact) { const btVector3* vertices = &m_triangle->getVertexPtr(0); @@ -92,25 +94,25 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); normal.normalize(); btVector3 p1ToCentre = c - vertices[0]; - float distanceFromPlane = p1ToCentre.dot(normal); + btScalar distanceFromPlane = p1ToCentre.dot(normal); - if (distanceFromPlane < 0.f) + if (distanceFromPlane < btScalar(0.)) { //triangle facing the other way - distanceFromPlane *= -1.f; - normal *= -1.f; + distanceFromPlane *= btScalar(-1.); + normal *= btScalar(-1.); } ///todo: move this gContactBreakingThreshold into a proper structure - extern float gContactBreakingThreshold; + extern btScalar gContactBreakingThreshold; - float contactMargin = gContactBreakingThreshold; + btScalar contactMargin = gContactBreakingThreshold; bool isInsideContactPlane = distanceFromPlane < r + contactMargin; bool isInsideShellPlane = distanceFromPlane < r; - float deltaDotNormal = delta.dot(normal); - if (!isInsideShellPlane && deltaDotNormal >= 0.0f) + btScalar deltaDotNormal = delta.dot(normal); + if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0)) return false; // Check for contact / intersection @@ -123,7 +125,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po contactPoint = c - normal*distanceFromPlane; } else { // Could be inside one of the contact capsules - float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); + btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); btVector3 nearestOnEdge; for (int i = 0; i < m_triangle->getNumEdges(); i++) { @@ -132,7 +134,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po m_triangle->getEdge(i,pa,pb); - float distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); + btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); if (distanceSqr < contactCapsuleRadiusSqr) { // Yep, we're inside a capsule hasContact = true; @@ -145,25 +147,22 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po if (hasContact) { btVector3 contactToCentre = c - contactPoint; - float distanceSqr = contactToCentre.length2(); + btScalar distanceSqr = contactToCentre.length2(); if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { - float distance = btSqrt(distanceSqr); - if (1) - { - resultNormal = contactToCentre; - resultNormal.normalize(); - } + btScalar distance = btSqrt(distanceSqr); + resultNormal = contactToCentre; + resultNormal.normalize(); point = contactPoint; depth = -(r-distance); return true; } - if (delta.dot(contactToCentre) >= 0.0f) + if (delta.dot(contactToCentre) >= btScalar(0.0)) return false; // Moving towards the contact point -> collision point = contactPoint; - timeOfImpact = 0.0f; + timeOfImpact = btScalar(0.0); return true; } @@ -189,7 +188,7 @@ bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const b btVector3 edge2_normal( edge2.cross(normal)); btVector3 edge3_normal( edge3.cross(normal)); - float r1, r2, r3; + btScalar r1, r2, r3; r1 = edge1_normal.dot( p1_to_p ); r2 = edge2_normal.dot( p2_to_p ); r3 = edge3_normal.dot( p3_to_p ); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 8fedba19922..b32806a6846 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef SPHERE_TRIANGLE_DETECTOR_H #define SPHERE_TRIANGLE_DETECTOR_H -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" +#include "../NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "../../LinearMath/btPoint3.h" class btSphereShape; @@ -36,7 +36,7 @@ struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface private: - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact); + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact); bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index 1eaa4a0fd50..d51a59af7f0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef COLLISION_CREATE_FUNC #define COLLISION_CREATE_FUNC -#include <vector> - -typedef std::vector<class btCollisionObject*> btCollisionObjectArray; +#include "../../LinearMath/btAlignedObjectArray.h" +typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; class btCollisionAlgorithm; class btCollisionObject; @@ -35,8 +34,11 @@ struct btCollisionAlgorithmCreateFunc } virtual ~btCollisionAlgorithmCreateFunc(){}; - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , btCollisionObject* body0,btCollisionObject* body1) { + + (void)body0; + (void)body1; return 0; } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 309c1890b40..198276d76df 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -25,7 +25,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include <algorithm> #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" int gNumManifold = 0; @@ -33,16 +32,17 @@ int gNumManifold = 0; #include <stdio.h> -btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms) -:m_useIslands(true), -m_convexConvexCreateFunc(0), +btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms): m_count(0), +m_useIslands(true), +m_convexConvexCreateFunc(0), m_convexConcaveCreateFunc(0), m_swappedConvexConcaveCreateFunc(0), m_compoundCreateFunc(0), m_swappedCompoundCreateFunc(0), m_emptyCreateFunc(0) { + (void)noDefaultAlgorithms; int i; setNearCallback(defaultNearCallback); @@ -56,11 +56,14 @@ m_emptyCreateFunc(0) } } } - +//if you want to not link with the default collision algorithms, you can +//define BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION +//in your Bullet library build system +#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION btCollisionDispatcher::btCollisionDispatcher (): - m_useIslands(true), - m_count(0) + m_count(0), + m_useIslands(true) { int i; @@ -86,6 +89,9 @@ btCollisionDispatcher::btCollisionDispatcher (): }; +#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION + + void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) { m_doubleDispatch[proxyType0][proxyType1] = createFunc; @@ -129,20 +135,17 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) gNumManifold--; //printf("releaseManifold: gNumManifold %d\n",gNumManifold); - clearManifold(manifold); - std::vector<btPersistentManifold*>::iterator i = - std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold); - if (!(i == m_manifoldsPtr.end())) + ///todo: this can be improved a lot, linear search might be slow part! + int findIndex = m_manifoldsPtr.findLinearSearch(manifold); + if (findIndex < m_manifoldsPtr.size()) { - std::swap(*i, m_manifoldsPtr.back()); + m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); m_manifoldsPtr.pop_back(); delete manifold; - } - } @@ -164,6 +167,8 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo } +#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION + btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1) { @@ -197,6 +202,8 @@ btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(in return m_emptyCreateFunc; } +#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION + #ifndef USE_DISPATCH_REGISTRY_ARRAY @@ -288,6 +295,16 @@ public: { } + btCollisionPairCallback& operator=(btCollisionPairCallback& other) + { + m_dispatchInfo = other.m_dispatchInfo; + m_dispatcher = other.m_dispatcher; + return *this; + } + + virtual ~btCollisionPairCallback() {} + + virtual bool processOverlap(btBroadphasePair& pair) { (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); @@ -337,7 +354,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, } else { //continuous collision detection query, time of impact (toi) - float toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); + btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); if (dispatchInfo.m_timeOfImpact > toi) dispatchInfo.m_timeOfImpact = toi; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 77c09b618ec..ca5aba8f01c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -16,13 +16,13 @@ subject to the following restrictions: #ifndef COLLISION__DISPATCHER_H #define COLLISION__DISPATCHER_H -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "../BroadphaseCollision/btDispatcher.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btManifoldResult.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../../LinearMath/btAlignedObjectArray.h" class btIDebugDraw; class btOverlappingPairCache; @@ -41,8 +41,9 @@ typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispa ///Time of Impact, Closest Points and Penetration Depth. class btCollisionDispatcher : public btDispatcher { + int m_count; - std::vector<btPersistentManifold*> m_manifoldsPtr; + btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; bool m_useIslands; @@ -68,7 +69,6 @@ class btCollisionDispatcher : public btDispatcher public: - ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); @@ -92,8 +92,6 @@ public: return m_manifoldsPtr[index]; } - int m_count; - ///the default constructor creates/register default collision algorithms, for convex, compound and concave shape support btCollisionDispatcher (); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 13fa0b5baa9..7b6f066275d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -20,11 +20,11 @@ btCollisionObject::btCollisionObject() m_collisionShape(0), m_collisionFlags(0), m_activationState1(1), - m_deactivationTime(0.f), + m_deactivationTime(btScalar(0.)), m_userObjectPointer(0), - m_hitFraction(1.f), - m_ccdSweptSphereRadius(0.f), - m_ccdSquareMotionThreshold(0.f) + m_hitFraction(btScalar(1.)), + m_ccdSweptSphereRadius(btScalar(0.)), + m_ccdSquareMotionThreshold(btScalar(0.)) { } @@ -46,7 +46,7 @@ void btCollisionObject::activate(bool forceActivation) if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) { setActivationState(ACTIVE_TAG); - m_deactivationTime = 0.f; + m_deactivationTime = btScalar(0.); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 9a0129ac29c..93990235afe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef COLLISION_OBJECT_H #define COLLISION_OBJECT_H -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btTransform.h" //island management, m_activationState1 #define ACTIVE_TAG 1 @@ -27,21 +27,19 @@ subject to the following restrictions: struct btBroadphaseProxy; class btCollisionShape; -#include "LinearMath/btMotionState.h" +#include "../../LinearMath/btMotionState.h" /// btCollisionObject can be used to manage collision detection objects. /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. /// They can be added to the btCollisionWorld. -class btCollisionObject +ATTRIBUTE_ALIGNED16(class) btCollisionObject { protected: btTransform m_worldTransform; - btBroadphaseProxy* m_broadphaseHandle; - btCollisionShape* m_collisionShape; ///m_interpolationWorldTransform is used for CCD and interpolation ///it can be either previous or future (predicted) transform @@ -50,12 +48,16 @@ protected: //without destroying the continuous interpolated motion (which uses this interpolation velocities) btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; + btBroadphaseProxy* m_broadphaseHandle; + btCollisionShape* m_collisionShape; int m_collisionFlags; int m_islandTag1; + int m_companionId; + int m_activationState1; - float m_deactivationTime; + btScalar m_deactivationTime; btScalar m_friction; btScalar m_restitution; @@ -67,13 +69,15 @@ protected: void* m_internalOwner; ///time of impact calculation - float m_hitFraction; + btScalar m_hitFraction; ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - float m_ccdSweptSphereRadius; + btScalar m_ccdSweptSphereRadius; /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - float m_ccdSquareMotionThreshold; + btScalar m_ccdSquareMotionThreshold; + + char m_pad[8]; public: @@ -82,7 +86,7 @@ public: CF_STATIC_OBJECT= 1, CF_KINEMATIC_OBJECT= 2, CF_NO_CONTACT_RESPONSE = 4, - CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution) }; @@ -137,11 +141,11 @@ public: void setActivationState(int newState); - void setDeactivationTime(float time) + void setDeactivationTime(btScalar time) { m_deactivationTime = time; } - float getDeactivationTime() const + btScalar getDeactivationTime() const { return m_deactivationTime; } @@ -155,19 +159,19 @@ public: return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } - void setRestitution(float rest) + void setRestitution(btScalar rest) { m_restitution = rest; } - float getRestitution() const + btScalar getRestitution() const { return m_restitution; } - void setFriction(float frict) + void setFriction(btScalar frict) { m_friction = frict; } - float getFriction() const + btScalar getFriction() const { return m_friction; } @@ -251,12 +255,22 @@ public: m_islandTag1 = tag; } - const float getHitFraction() const + const int getCompanionId() const + { + return m_companionId; + } + + void setCompanionId(int id) + { + m_companionId = id; + } + + const btScalar getHitFraction() const { return m_hitFraction; } - void setHitFraction(float hitFraction) + void setHitFraction(btScalar hitFraction) { m_hitFraction = hitFraction; } @@ -273,25 +287,25 @@ public: } ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - float getCcdSweptSphereRadius() const + btScalar getCcdSweptSphereRadius() const { return m_ccdSweptSphereRadius; } ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - void setCcdSweptSphereRadius(float radius) + void setCcdSweptSphereRadius(btScalar radius) { m_ccdSweptSphereRadius = radius; } - float getCcdSquareMotionThreshold() const + btScalar getCcdSquareMotionThreshold() const { return m_ccdSquareMotionThreshold; } /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - void setCcdSquareMotionThreshold(float ccdSquareMotionThreshold) + void setCcdSquareMotionThreshold(btScalar ccdSquareMotionThreshold) { m_ccdSquareMotionThreshold = ccdSquareMotionThreshold; } @@ -308,6 +322,7 @@ public: m_userObjectPointer = userPointer; } -}; +} +; #endif //COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index fa1561973fb..49f35c2cc90 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -31,7 +31,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include <algorithm> btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache, int stackSize) :m_dispatcher1(dispatcher), @@ -50,13 +49,10 @@ btCollisionWorld::~btCollisionWorld() delete m_stackAlloc; //clean up remaining objects - std::vector<btCollisionObject*>::iterator i; - - for (i=m_collisionObjects.begin(); - !(i==m_collisionObjects.end()); i++) - + int i; + for (i=0;i<m_collisionObjects.size();i++) { - btCollisionObject* collisionObject= (*i); + btCollisionObject* collisionObject= m_collisionObjects[i]; btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) @@ -89,9 +85,7 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho { //check that the object isn't already added - std::vector<btCollisionObject*>::iterator i = std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject); - assert(i == m_collisionObjects.end()); - + btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); m_collisionObjects.push_back(collisionObject); @@ -125,13 +119,13 @@ void btCollisionWorld::performDiscreteCollisionDetection() { btDispatcherInfo& dispatchInfo = getDispatchInfo(); - BEGIN_PROFILE("performDiscreteCollisionDetection"); + BEGIN_PROFILE("perform Broadphase Collision Detection"); //update aabb (of all moved objects) btVector3 aabbMin,aabbMax; - for (size_t i=0;i<m_collisionObjects.size();i++) + for (int i=0;i<m_collisionObjects.size();i++) { m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax); m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax); @@ -139,6 +133,11 @@ void btCollisionWorld::performDiscreteCollisionDetection() m_broadphasePairCache->refreshOverlappingPairs(); + + END_PROFILE("perform Broadphase Collision Detection"); + + BEGIN_PROFILE("performDiscreteCollisionDetection"); + btDispatcher* dispatcher = getDispatcher(); if (dispatcher) dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache,dispatchInfo); @@ -169,15 +168,13 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) } - std::vector<btCollisionObject*>::iterator i = std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject); - - if (!(i == m_collisionObjects.end())) - { - std::swap(*i, m_collisionObjects.back()); - m_collisionObjects.pop_back(); - } + //swapremove + m_collisionObjects.remove(collisionObject); + } + + void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, @@ -185,12 +182,13 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra RayResultCallback& resultCallback) { - btSphereShape pointShape(0.0f); + btSphereShape pointShape(btScalar(0.0)); + pointShape.setMargin(0.f); if (collisionShape->isConvex()) { btConvexCast::CastResult castResult; - castResult.m_fraction = 1.f;//?? + castResult.m_fraction = btScalar(1.);//?? btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; @@ -201,7 +199,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit - if (castResult.m_normal.length2() > 0.0001f) + if (castResult.m_normal.length2() > btScalar(0.0001)) { castResult.m_normal.normalize(); if (castResult.m_fraction < resultCallback.m_closestHitFraction) @@ -252,7 +250,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } - virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; @@ -321,19 +319,16 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) - std::vector<btCollisionObject*>::iterator iter; - - for (iter=m_collisionObjects.begin(); - !(iter==m_collisionObjects.end()); iter++) + int i; + for (i=0;i<m_collisionObjects.size();i++) { - - btCollisionObject* collisionObject= (*iter); + btCollisionObject* collisionObject= m_collisionObjects[i]; //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - float hitLambda = 1.f; //could use resultCallback.m_closestHitFraction, but needs testing + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index bd09d8c4d5d..0b0286c50f4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -67,15 +67,12 @@ subject to the following restrictions: class btStackAlloc; class btCollisionShape; class btBroadphaseInterface; -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" #include "btCollisionObject.h" #include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" - -#include <vector> - - +#include "../BroadphaseCollision/btOverlappingPairCache.h" +#include "../../LinearMath/btAlignedObjectArray.h" ///CollisionWorld is interface and container for the collision detection class btCollisionWorld @@ -84,7 +81,7 @@ class btCollisionWorld protected: - std::vector<btCollisionObject*> m_collisionObjects; + btAlignedObjectArray<btCollisionObject*> m_collisionObjects; btDispatcher* m_dispatcher1; @@ -137,18 +134,18 @@ public: LocalRayResult(btCollisionObject* collisionObject, LocalShapeInfo* localShapeInfo, const btVector3& hitNormalLocal, - float hitFraction) + btScalar hitFraction) :m_collisionObject(collisionObject), - m_localShapeInfo(m_localShapeInfo), + m_localShapeInfo(localShapeInfo), m_hitNormalLocal(hitNormalLocal), m_hitFraction(hitFraction) { } - btCollisionObject* m_collisionObject; + btCollisionObject* m_collisionObject; LocalShapeInfo* m_localShapeInfo; - const btVector3& m_hitNormalLocal; - float m_hitFraction; + btVector3 m_hitNormalLocal; + btScalar m_hitFraction; }; @@ -158,17 +155,17 @@ public: virtual ~RayResultCallback() { } - float m_closestHitFraction; + btScalar m_closestHitFraction; bool HasHit() { - return (m_closestHitFraction < 1.f); + return (m_closestHitFraction < btScalar(1.)); } RayResultCallback() - :m_closestHitFraction(1.f) + :m_closestHitFraction(btScalar(1.)) { } - virtual float AddSingleResult(LocalRayResult& rayResult) = 0; + virtual btScalar AddSingleResult(LocalRayResult& rayResult) = 0; }; struct ClosestRayResultCallback : public RayResultCallback @@ -187,7 +184,7 @@ public: btVector3 m_hitPointWorld; btCollisionObject* m_collisionObject; - virtual float AddSingleResult(LocalRayResult& rayResult) + virtual btScalar AddSingleResult(LocalRayResult& rayResult) { //caller already does the filter on the m_closestHitFraction @@ -216,7 +213,7 @@ public: /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. /// This allows more customization. - void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 43887d26a45..2f96c2e13c5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -89,7 +89,7 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt } } -float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { btCollisionObject* colObj = m_isSwapped? body1 : body0; @@ -106,7 +106,7 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod //then use each overlapping node AABB against Tree0 //and vise versa. - float hitFraction = 1.f; + btScalar hitFraction = btScalar(1.); int numChildren = m_childCollisionAlgorithms.size(); int i; @@ -124,7 +124,7 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod colObj->setWorldTransform( newChildWorldTrans ); colObj->setCollisionShape( childShape ); - float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); + btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); if (frac<hitFraction) { hitFraction = frac; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index fe2d8628656..7091b233b46 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -16,21 +16,21 @@ subject to the following restrictions: #ifndef COMPOUND_COLLISION_ALGORITHM_H #define COMPOUND_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btDispatcher.h" +#include "../BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include <vector> +#include "../BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" +#include "../../LinearMath/btAlignedObjectArray.h" /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes /// Place holder, not fully implemented yet class btCompoundCollisionAlgorithm : public btCollisionAlgorithm { - std::vector<btCollisionAlgorithm*> m_childCollisionAlgorithms; + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; public: @@ -41,7 +41,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); struct CreateFunc :public btCollisionAlgorithmCreateFunc { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 06adb3a04c8..24ceacfd40d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -95,7 +95,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); //btVector3 center = triangle[0] + triangle[1]+triangle[2]; - //center *= 0.333333f; + //center *= btScalar(0.333333); //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); @@ -134,7 +134,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i -void btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { m_dispatchInfoPtr = &dispatchInfo; m_collisionMarginTriangle = collisionMarginTriangle; @@ -146,7 +146,7 @@ void btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTrian btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape()); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - float extraMargin = collisionMarginTriangle; + btScalar extraMargin = collisionMarginTriangle; btVector3 extra(extraMargin,extraMargin,extraMargin); m_aabbMax += extra; @@ -176,7 +176,7 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod if (convexBody->getCollisionShape()->isConvex()) { - float collisionMarginTriangle = concaveShape->getMargin(); + btScalar collisionMarginTriangle = concaveShape->getMargin(); resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); @@ -196,9 +196,10 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod } -float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - + (void)resultOut; + (void)dispatchInfo; btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; @@ -207,10 +208,10 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject //only perform CCD above a certain threshold, this prevents blocking on the long run //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - float squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); + btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) { - return 1.f; + return btScalar(1.); } //const btVector3& from = convexbody->m_worldTransform.getOrigin(); @@ -227,11 +228,11 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject btTransform m_ccdSphereToTrans; btTransform m_meshTransform; - float m_ccdSphereRadius; - float m_hitFraction; + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,float ccdSphereRadius,float hitFraction) + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) :m_ccdSphereFromTrans(from), m_ccdSphereToTrans(to), m_ccdSphereRadius(ccdSphereRadius), @@ -242,6 +243,8 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + (void)partId; + (void)triangleIndex; //do a swept sphere for now btTransform ident; ident.setIdentity(); @@ -276,11 +279,11 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject rayAabbMin.setMin(convexToLocal.getOrigin()); btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); - float ccdRadius0 = convexbody->getCcdSweptSphereRadius(); + btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - float curHitFraction = 1.f; //is this available? + btScalar curHitFraction = btScalar(1.); //is this available? LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, convexbody->getCcdSweptSphereRadius(),curHitFraction); @@ -304,6 +307,6 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject } } - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index afcb38c94ef..4915b6c20c8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -16,13 +16,13 @@ subject to the following restrictions: #ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H #define CONVEX_CONCAVE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btDispatcher.h" +#include "../BroadphaseCollision/btBroadphaseInterface.h" +#include "../CollisionShapes/btTriangleCallback.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. @@ -38,7 +38,7 @@ class btConvexTriangleCallback : public btTriangleCallback btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; - float m_collisionMarginTriangle; + btScalar m_collisionMarginTriangle; public: int m_triangleCount; @@ -47,7 +47,7 @@ int m_triangleCount; btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); - void setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual ~btConvexTriangleCallback(); @@ -86,7 +86,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); void clearCache(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 09457aea3e9..9105fe20b49 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -43,23 +43,9 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#ifdef WIN32 -#if _MSC_VER >= 1310 -//only use SIMD Hull code under Win32 -#ifdef TEST_HULL -#define USE_HULL 1 -#endif //TEST_HULL -#endif //_MSC_VER -#endif //WIN32 -#ifdef USE_HULL -#include "NarrowPhaseCollision/Hull.h" -#include "NarrowPhaseCollision/HullContactCollector.h" - - -#endif //USE_HULL btConvexConvexAlgorithm::CreateFunc::CreateFunc() @@ -76,6 +62,14 @@ btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simp m_pdSolver = pdSolver; } +btConvexConvexAlgorithm::CreateFunc::~CreateFunc() +{ + if (m_ownsSolvers){ + delete m_simplexSolver; + delete m_pdSolver; + } +} + btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) : btCollisionAlgorithm(ci), m_gjkPairDetector(0,0,simplexSolver,pdSolver), @@ -83,6 +77,9 @@ m_ownManifold (false), m_manifoldPtr(mf), m_lowLevelOfDetail(false) { + (void)body0; + (void)body1; + } @@ -147,7 +144,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; input.m_stackAlloc = dispatchInfo.m_stackAllocator; -// input.m_maximumDistanceSquared = 1e30f; +// input.m_maximumDistanceSquared = btScalar(1e30); input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); @@ -160,24 +157,26 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl bool disableCcd = false; -float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)resultOut; + (void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, - float resultFraction = 1.f; + btScalar resultFraction = btScalar(1.); - float squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); - float squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); + btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); + btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); if (squareMot0 < col0->getCcdSquareMotionThreshold() && squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; if (disableCcd) - return 1.f; + return btScalar(1.); //An adhoc way of testing the Continuous Collision Detection algorithms diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index ccfe6d5b8cb..cbea9a92b75 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -16,11 +16,11 @@ subject to the following restrictions: #ifndef CONVEX_CONVEX_ALGORITHM_H #define CONVEX_CONVEX_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../NarrowPhaseCollision/btGjkPairDetector.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" class btConvexPenetrationDepthSolver; @@ -44,7 +44,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); void setLowLevelOfDetail(bool useLowLevel); @@ -62,6 +62,7 @@ public: CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); CreateFunc(); + virtual ~CreateFunc(); virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp index 9bc106564af..936054387c4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -22,14 +22,13 @@ btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& c { } -void btEmptyAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btEmptyAlgorithm::processCollision (btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) { - } -float btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) { - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index e0e136250ac..b1a193d2cfd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -15,7 +15,7 @@ subject to the following restrictions: #ifndef EMPTY_ALGORITH #define EMPTY_ALGORITH -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" #include "btCollisionCreateFunc.h" #define ATTRIBUTE_ALIGNED(a) @@ -31,12 +31,14 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { + (void)body0; + (void)body1; return new btEmptyAlgorithm(ci); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index cd22f3dd91e..490acc0b611 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -27,7 +27,7 @@ inline btScalar calculateCombinedFriction(const btCollisionObject* body0,const b { btScalar friction = body0->getFriction() * body1->getFriction(); - const btScalar MAX_FRICTION = 10.f; + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) @@ -53,7 +53,7 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b } -void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) +void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { assert(m_manifoldPtr); //order in manifold needs to match @@ -63,15 +63,22 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b bool isSwapped = m_manifoldPtr->getBody0() != m_body0; - btTransform transAInv = isSwapped? m_rootTransB.inverse() : m_rootTransA.inverse(); - btTransform transBInv = isSwapped? m_rootTransA.inverse() : m_rootTransB.inverse(); - btVector3 pointA = pointInWorld + normalOnBInWorld * depth; - btVector3 localA = transAInv(pointA ); - btVector3 localB = transBInv(pointInWorld); - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + btVector3 localA; + btVector3 localB; + if (isSwapped) + { + localA = m_rootTransB.invXform(pointA ); + localB = m_rootTransA.invXform(pointInWorld); + } else + { + localA = m_rootTransA.invXform(pointA ); + localB = m_rootTransB.invXform(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); int insertIndex = m_manifoldPtr->getCacheEntry(newPt); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 3d70689e3eb..77192625513 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -23,7 +23,7 @@ class btManifoldPoint; #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btTransform.h" typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1); extern ContactAddedCallback gContactAddedCallback; @@ -68,7 +68,7 @@ public: m_index1=index1; } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth); + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 06a5f95143e..ac2e8554c3a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -8,8 +8,7 @@ #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include <stdio.h> -#include <algorithm> - +#include "LinearMath/btQuickprof.h" btSimulationIslandManager::btSimulationIslandManager() { @@ -33,7 +32,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher) for (int i=0;i<dispatcher->getNumManifolds();i++) { const btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); - //static objects (invmass 0.f) don't merge ! + //static objects (invmass btScalar(0.)) don't merge ! const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); @@ -57,16 +56,15 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld // put the index into m_controllers into m_tag { - std::vector<btCollisionObject*>::iterator i; int index = 0; - for (i=colWorld->getCollisionObjectArray().begin(); - !(i==colWorld->getCollisionObjectArray().end()); i++) + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) { - - btCollisionObject* collisionObject= (*i); + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; collisionObject->setIslandTag(index); - collisionObject->setHitFraction(1.f); + collisionObject->setCompanionId(-1); + collisionObject->setHitFraction(btScalar(1.)); index++; } @@ -88,20 +86,19 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col { - std::vector<btCollisionObject*>::iterator i; - int index = 0; - for (i=colWorld->getCollisionObjectArray().begin(); - !(i==colWorld->getCollisionObjectArray().end()); i++) + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size();i++) { - btCollisionObject* collisionObject= (*i); - + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; if (collisionObject->mergesSimulationIslands()) { collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setCompanionId(-1); } else { collisionObject->setIslandTag(-1); + collisionObject->setCompanionId(-2); } index++; } @@ -118,13 +115,21 @@ inline int getIslandId(const btPersistentManifold* lhs) } -bool btPersistentManifoldSortPredicate(const btPersistentManifold* lhs, const btPersistentManifold* rhs) + + +/// function object that routes calls to operator< +class btPersistentManifoldSortPredicate { - int rIslandId0,lIslandId0; - rIslandId0 = getIslandId(rhs); - lIslandId0 = getIslandId(lhs); - return lIslandId0 < rIslandId0; -} + public: + + SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) + { + return getIslandId(lhs) < getIslandId(rhs); + } +}; + + + // @@ -132,6 +137,22 @@ bool btPersistentManifoldSortPredicate(const btPersistentManifold* lhs, const bt // void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) { + + + + /*if (0) + { + int maxNumManifolds = dispatcher->getNumManifolds(); + btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher; + btPersistentManifold** manifold = colDis->getInternalManifoldPointer(); + callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0); + return; + } + */ + + + BEGIN_PROFILE("islandUnionFindAndHeapSort"); + //we are going to sort the unionfind array, and store the element id in the size //afterwards, we clean unionfind, to make sure no-one uses it anymore @@ -139,9 +160,11 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int numElem = getUnionFind().getNumElements(); int endIslandIndex=1; + int startIslandIndex; + //update the sleeping state for bodies, if all are sleeping - for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) { int islandId = getUnionFind().getElement(startIslandIndex).m_id; for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) @@ -224,7 +247,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, } } - std::vector<btPersistentManifold*> islandmanifold; + btAlignedObjectArray<btPersistentManifold*> islandmanifold; int i; int maxNumManifolds = dispatcher->getNumManifolds(); islandmanifold.reserve(maxNumManifolds); @@ -261,63 +284,74 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, // Sort manifolds, based on islands // Sort the vector using predicate and std::sort - std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + + //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) int startManifoldIndex = 0; int endManifoldIndex = 1; - int islandId; + //int islandId; + + END_PROFILE("islandUnionFindAndHeapSort"); + + btAlignedObjectArray<btCollisionObject*> islandBodies; - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) - { - int islandId = getUnionFind().getElement(startIslandIndex).m_id; - - - bool islandSleeping = false; + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + + + bool islandSleeping = false; for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) { int i = getUnionFind().getElement(endIslandIndex).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; + islandBodies.push_back(colObj0); if (!colObj0->isActive()) islandSleeping = true; } - //find the accompanying contact manifold for this islandId - int numIslandManifolds = 0; - btPersistentManifold** startManifold = 0; + //find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + btPersistentManifold** startManifold = 0; - if (startManifoldIndex<numManifolds) - { - int curIslandId = getIslandId(islandmanifold[startManifoldIndex]); - if (curIslandId == islandId) - { - startManifold = &islandmanifold[startManifoldIndex]; - - for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++) - { + if (startManifoldIndex<numManifolds) + { + int curIslandId = getIslandId(islandmanifold[startManifoldIndex]); + if (curIslandId == islandId) + { + startManifold = &islandmanifold[startManifoldIndex]; + + for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++) + { - } - /// Process the actual simulation, only if not sleeping/deactivated - numIslandManifolds = endManifoldIndex-startManifoldIndex; - } + } + /// Process the actual simulation, only if not sleeping/deactivated + numIslandManifolds = endManifoldIndex-startManifoldIndex; + } - } + } - if (!islandSleeping) - { - callback->ProcessIsland(startManifold,numIslandManifolds, islandId); - } + if (!islandSleeping) + { + callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId); + } + + if (numIslandManifolds) + { + startManifoldIndex = endManifoldIndex; + } - if (numIslandManifolds) - { - startManifoldIndex = endManifoldIndex; - } - } + islandBodies.resize(0); + } + } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 68d9b8038d6..d91ed1c20eb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -16,9 +16,10 @@ subject to the following restrictions: #ifndef SIMULATION_ISLAND_MANAGER_H #define SIMULATION_ISLAND_MANAGER_H -#include "BulletCollision/CollisionDispatch/btUnionFind.h" +#include "../CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" +class btCollisionObject; class btCollisionWorld; class btDispatcher; @@ -49,7 +50,7 @@ public: { virtual ~IslandCallback() {}; - virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 1423c335407..05556bd34e2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -50,6 +50,8 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)dispatchInfo; + (void)resultOut; if (!m_manifoldPtr) return; @@ -64,7 +66,7 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin(); btScalar radius = sphere0->getRadius(); - float dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); + btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); if (dist < SIMD_EPSILON) { @@ -81,10 +83,15 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b } -float btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + //not yet - return 1.f; + return btScalar(1.); } @@ -117,14 +124,14 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box ///////////////////////////////////////////////// btVector3 tmp, prel, n[6], normal, v3P; - btScalar fSep = 10000000.0f, fSepThis; + btScalar fSep = btScalar(10000000.0), fSepThis; - n[0].setValue( -1.0f, 0.0f, 0.0f ); - n[1].setValue( 0.0f, -1.0f, 0.0f ); - n[2].setValue( 0.0f, 0.0f, -1.0f ); - n[3].setValue( 1.0f, 0.0f, 0.0f ); - n[4].setValue( 0.0f, 1.0f, 0.0f ); - n[5].setValue( 0.0f, 0.0f, 1.0f ); + n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); + n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); + n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); + n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); + n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); + n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); // convert point in local space prel = m44T.invXform( sphereCenter); @@ -136,7 +143,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box for (int i=0;i<6;i++) { int j = i<3? 0:1; - if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > 0.0f ) + if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) ) { v3P = v3P - n[i]*fSepThis; bFound = true; @@ -154,9 +161,9 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box pointOnBox = v3P + normal*margins; v3PointOnSphere = prel - normal*fRadius; - if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > 0.0f ) + if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) ) { - return 1.0f; + return btScalar(1.0); } // transform back in world space @@ -171,7 +178,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box { fSep = - btSqrt(fSeps2); normal = (pointOnBox-v3PointOnSphere); - normal *= 1.f/fSep; + normal *= btScalar(1.)/fSep; } return fSep; @@ -185,10 +192,10 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box bounds[0] = boundsVec[0]; bounds[1] = boundsVec[1]; - if ( fPenetration <= 0.0f ) + if ( fPenetration <= btScalar(0.0) ) return (fPenetration-margins); else - return 1.0f; + return btScalar(1.0); } btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) @@ -200,14 +207,14 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* bounds[1] = aabbMax; btVector3 p0, tmp, prel, n[6], normal; - btScalar fSep = -10000000.0f, fSepThis; + btScalar fSep = btScalar(-10000000.0), fSepThis; - n[0].setValue( -1.0f, 0.0f, 0.0f ); - n[1].setValue( 0.0f, -1.0f, 0.0f ); - n[2].setValue( 0.0f, 0.0f, -1.0f ); - n[3].setValue( 1.0f, 0.0f, 0.0f ); - n[4].setValue( 0.0f, 1.0f, 0.0f ); - n[5].setValue( 0.0f, 0.0f, 1.0f ); + n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); + n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); + n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); + n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); + n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); + n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); const btTransform& m44T = boxObj->getWorldTransform(); @@ -219,7 +226,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* for (int i=0;i<6;i++) { int j = i<3 ? 0:1; - if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > 0.0f ) return 1.0f; + if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) ) return btScalar(1.0); if ( fSepThis > fSep ) { p0 = bounds[j]; normal = (btVector3&)n[i]; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 68915a43e6f..07592909200 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -16,11 +16,11 @@ subject to the following restrictions: #ifndef SPHERE_BOX_COLLISION_ALGORITHM_H #define SPHERE_BOX_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. /// Other features are frame-coherency (persistent data) and collision response. @@ -38,7 +38,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius ); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index eb05bf974c5..424ff432f84 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -41,6 +41,8 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)dispatchInfo; + if (!m_manifoldPtr) return; @@ -48,7 +50,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape(); btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin(); - float len = diff.length(); + btScalar len = diff.length(); btScalar radius0 = sphere0->getRadius(); btScalar radius1 = sphere1->getRadius(); @@ -71,8 +73,13 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 } -float btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)col0; + (void)col1; + (void)dispatchInfo; + (void)resultOut; + //not yet - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 8b08d015aec..7a19ff31edf 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -16,9 +16,9 @@ subject to the following restrictions: #ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H #define SPHERE_SPHERE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. @@ -37,7 +37,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual ~btSphereSphereCollisionAlgorithm(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 6f74c488659..b011b707e3f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co SphereTriangleDetector detector(sphere,triangle); btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = 1e30f;//todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds input.m_transformA = col0->getWorldTransform(); input.m_transformB = col1->getWorldTransform(); @@ -64,8 +64,13 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co } -float btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + //not yet - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 82e4c5b37f1..57c6e6af619 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -16,9 +16,9 @@ subject to the following restrictions: #ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. @@ -38,7 +38,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual ~btSphereTriangleCollisionAlgorithm(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 046f348a147..62254335796 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -15,7 +15,6 @@ subject to the following restrictions: #include "btUnionFind.h" #include <assert.h> -#include <algorithm> @@ -50,11 +49,16 @@ void btUnionFind::reset(int N) } } -bool btUnionFindElementSortPredicate(const btElement& lhs, const btElement& rhs) + +class btUnionFindElementSortPredicate { - return lhs.m_id < rhs.m_id; -} + public: + bool operator() ( const btElement& lhs, const btElement& rhs ) + { + return lhs.m_id < rhs.m_id; + } +}; ///this is a special operation, destroying the content of btUnionFind. ///it sorts the elements, based on island id, in order to make it easy to iterate over islands @@ -71,7 +75,9 @@ void btUnionFind::sortIslands() } // Sort the vector using predicate and std::sort - std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + //perhaps use radix sort? + m_elements.heapSort(btUnionFindElementSortPredicate()); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index b1baca9ff15..236cc33b94f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -16,7 +16,10 @@ subject to the following restrictions: #ifndef UNION_FIND_H #define UNION_FIND_H -#include <vector> +#include "../../LinearMath/btAlignedObjectArray.h" + + #define USE_PATH_COMPRESSION 1 + struct btElement { int m_id; @@ -29,7 +32,7 @@ struct btElement class btUnionFind { private: - std::vector<btElement> m_elements; + btAlignedObjectArray<btElement> m_elements; public: @@ -78,6 +81,7 @@ class btUnionFind if (i == j) return; +#ifndef USE_PATH_COMPRESSION //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) if (m_elements[i].m_sz < m_elements[j].m_sz) { @@ -87,6 +91,9 @@ class btUnionFind { m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; } +#else + m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; +#endif //USE_PATH_COMPRESSION } int find(int x) @@ -97,7 +104,7 @@ class btUnionFind while (x != m_elements[x].m_id) { //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically - #define USE_PATH_COMPRESSION 1 + #ifdef USE_PATH_COMPRESSION // m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index b5f80de4557..636b0046c13 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -42,17 +42,16 @@ void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabb void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { - //float margin = 0.f; + //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtents(); - btScalar lx=2.f*(halfExtents.x()); - btScalar ly=2.f*(halfExtents.y()); - btScalar lz=2.f*(halfExtents.z()); - - inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); - inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); - inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + btScalar lx=btScalar(2.)*(halfExtents.x()); + btScalar ly=btScalar(2.)*(halfExtents.y()); + btScalar lz=btScalar(2.)*(halfExtents.z()); + inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + ly*ly)); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index ee451a9b6c0..bc42f146c7c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -17,10 +17,10 @@ subject to the following restrictions: #define OBB_BOX_MINKOWSKI_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "LinearMath/btPoint3.h" -#include "LinearMath/btSimdMinMax.h" +#include "btCollisionMargin.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btSimdMinMax.h" ///btBoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box class btBoxShape: public btPolyhedralConvexShape @@ -41,9 +41,9 @@ public: btVector3 halfExtents = getHalfExtents(); btVector3 supVertex; - supVertex = btPoint3(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), - vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + supVertex = btPoint3(vec.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); return supVertex; } @@ -54,9 +54,9 @@ public: btVector3 margin(getMargin(),getMargin(),getMargin()); halfExtents -= margin; - return btVector3(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), - vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + return btVector3(vec.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); } virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const @@ -69,9 +69,9 @@ public: for (int i=0;i<numVectors;i++) { const btVector3& vec = vectors[i]; - supportVerticesOut[i].setValue(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), - vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + supportVerticesOut[i].setValue(vec.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); } } @@ -132,27 +132,27 @@ public: switch (i) { case 0: - plane.setValue(1.f,0.f,0.f); + plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); plane[3] = -halfExtents.x(); break; case 1: - plane.setValue(-1.f,0.f,0.f); + plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); plane[3] = -halfExtents.x(); break; case 2: - plane.setValue(0.f,1.f,0.f); + plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); plane[3] = -halfExtents.y(); break; case 3: - plane.setValue(0.f,-1.f,0.f); + plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); plane[3] = -halfExtents.y(); break; case 4: - plane.setValue(0.f,0.f,1.f); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); plane[3] = -halfExtents.z(); break; case 5: - plane.setValue(0.f,0.f,-1.f); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); plane[3] = -halfExtents.z(); break; default: @@ -265,22 +265,22 @@ public: switch (index) { case 0: - penetrationVector.setValue(1.f,0.f,0.f); + penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); break; case 1: - penetrationVector.setValue(-1.f,0.f,0.f); + penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); break; case 2: - penetrationVector.setValue(0.f,1.f,0.f); + penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); break; case 3: - penetrationVector.setValue(0.f,-1.f,0.f); + penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); break; case 4: - penetrationVector.setValue(0.f,0.f,1.f); + penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); break; case 5: - penetrationVector.setValue(0.f,0.f,-1.f); + penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); break; default: assert(0); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 3bd0a317545..8da554ef14d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -18,21 +18,53 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btOptimizedBvh.h" + ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. -btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface) -:btTriangleMeshShape(meshInterface) +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression) +:btTriangleMeshShape(meshInterface),m_useQuantizedAabbCompression(useQuantizedAabbCompression) { //construct bvh from meshInterface #ifndef DISABLE_BVH m_bvh = new btOptimizedBvh(); - m_bvh->build(meshInterface); + btVector3 bvhAabbMin,bvhAabbMax; + meshInterface->calculateAabbBruteForce(bvhAabbMin,bvhAabbMax); + m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); #endif //DISABLE_BVH } +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) +:btTriangleMeshShape(meshInterface),m_useQuantizedAabbCompression(useQuantizedAabbCompression) +{ + //construct bvh from meshInterface +#ifndef DISABLE_BVH + + m_bvh = new btOptimizedBvh(); + m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + +#endif //DISABLE_BVH + +} + +void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax) +{ + m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax ); + + m_localAabbMin.setMin(aabbMin); + m_localAabbMax.setMax(aabbMax); +} + + +void btBvhTriangleMeshShape::refitTree() +{ + m_bvh->refit( m_meshInterface ); + + recalcLocalAabb(); +} + btBvhTriangleMeshShape::~btBvhTriangleMeshShape() { delete m_bvh; @@ -63,7 +95,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co { } - virtual void processNode(const btOptimizedBvhNode* node) + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { const unsigned char *vertexbase; int numverts; @@ -84,19 +116,21 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co indexstride, numfaces, indicestype, - node->m_subPart); + nodeSubPart); - int* gfxbase = (int*)(indexbase+node->m_triangleIndex*indexstride); - + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j=2;j>=0;j--) { int graphicsindex = gfxbase[j]; + + #ifdef DEBUG_TRIANGLE_MESH printf("%d ,",graphicsindex); #endif //DEBUG_TRIANGLE_MESH - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); m_triangle[j] = btVector3( graphicsbase[0]*meshScaling.getX(), @@ -107,8 +141,8 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co #endif //DEBUG_TRIANGLE_MESH } - m_callback->processTriangle(m_triangle,node->m_subPart,node->m_triangleIndex); - m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart); + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; @@ -130,8 +164,10 @@ void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { btTriangleMeshShape::setLocalScaling(scaling); delete m_bvh; + ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work m_bvh = new btOptimizedBvh(); - m_bvh->build(m_meshInterface); //rebuild the bvh... + m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax); + } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 59a27e8641a..4914d9f959c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -16,20 +16,26 @@ subject to the following restrictions: #ifndef BVH_TRIANGLE_MESH_SHAPE_H #define BVH_TRIANGLE_MESH_SHAPE_H -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" +#include "btTriangleMeshShape.h" +#include "btOptimizedBvh.h" ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. -class btBvhTriangleMeshShape : public btTriangleMeshShape +ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape { btOptimizedBvh* m_bvh; - - + bool m_useQuantizedAabbCompression; + bool m_pad[12];////need padding due to alignment + public: - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface); + btBvhTriangleMeshShape() :btTriangleMeshShape(0) {}; + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression); + + ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); + virtual ~btBvhTriangleMeshShape(); @@ -44,6 +50,10 @@ public: virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void refitTree(); + + ///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks + void partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax); //debugging virtual char* getName()const {return "BVHTRIANGLEMESH";} @@ -51,8 +61,15 @@ public: virtual void setLocalScaling(const btVector3& scaling); - - -}; + btOptimizedBvh* getOptimizedBvh() + { + return m_bvh; + } + bool usesQuantizedAabbCompression() const + { + return m_useQuantizedAabbCompression; + } +} +; #endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp new file mode 100644 index 00000000000..b7e15172da2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -0,0 +1,146 @@ +/* +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 "btCapsuleShape.h" + +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "LinearMath/btQuaternion.h" + +btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) +{ + m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); +} + + + btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + + btVector3 supVec(0,0,0); + + btScalar maxDot(btScalar(-1e30)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + btVector3 vtx; + btScalar newDot; + + btScalar radius = getRadius(); + + + { + btVector3 pos(0,getHalfHeight(),0); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0,-getHalfHeight(),0); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + +} + + void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + + + btScalar radius = getRadius(); + + for (int j=0;j<numVectors;j++) + { + btScalar maxDot(btScalar(-1e30)); + const btVector3& vec = vectors[j]; + + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0,getHalfHeight(),0); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + { + btVector3 pos(0,-getHalfHeight(),0); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + + } +} + + +void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //as an approximation, take the inertia of the box that bounds the spheres + + btTransform ident; + ident.setIdentity(); + + + btScalar radius = getRadius(); + + btVector3 halfExtents(radius,radius+getHalfHeight(),radius); + + btScalar margin = CONVEX_DISTANCE_MARGIN; + + btScalar lx=btScalar(2.)*(halfExtents[0]+margin); + btScalar ly=btScalar(2.)*(halfExtents[1]+margin); + btScalar lz=btScalar(2.)*(halfExtents[2]+margin); + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * btScalar(.08333333); + + inertia[0] = scaledmass * (y2+z2); + inertia[1] = scaledmass * (x2+z2); + inertia[2] = scaledmass * (x2+y2); + +} + + + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h new file mode 100644 index 00000000000..27da8adefa5 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -0,0 +1,60 @@ +/* +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_CAPSULE_SHAPE_H +#define BT_CAPSULE_SHAPE_H + +#include "btConvexShape.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types + + +///btCapsuleShape represents a capsule around the Y axis +///A more general solution that can represent capsules is the btMultiSphereShape +class btCapsuleShape : public btConvexShape +{ + +public: + btCapsuleShape(btScalar radius,btScalar height); + + ///CollisionShape Interface + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + /// btConvexShape Interface + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual int getShapeType() const { return CAPSULE_SHAPE_PROXYTYPE; } + + virtual char* getName()const + { + return "CapsuleShape"; + } + + btScalar getRadius() const + { + return m_implicitShapeDimensions.getX(); + } + + btScalar getHalfHeight() const + { + return m_implicitShapeDimensions.getY(); + } + +}; + + + +#endif //BT_CAPSULE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h index 377f0e506a2..4730264d3df 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -18,7 +18,7 @@ subject to the following restrictions: //used by Gjk and some other algorithms -#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f +#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 5474a201c37..81d82428f4c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -15,6 +15,16 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionShape.h" + +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" void btBulletCollisionProbe () {} + + + void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const { btTransform tr; @@ -23,14 +33,14 @@ void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) con getAabb(tr,aabbMin,aabbMax); - radius = (aabbMax-aabbMin).length()*0.5f; - center = (aabbMin+aabbMax)*0.5f; + radius = (aabbMax-aabbMin).length()*btScalar(0.5); + center = (aabbMin+aabbMax)*btScalar(0.5); } -float btCollisionShape::getAngularMotionDisc() const +btScalar btCollisionShape::getAngularMotionDisc() const { btVector3 center; - float disc; + btScalar disc; getBoundingSphere(center,disc); disc += (center).length(); return disc; @@ -41,25 +51,25 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b //start with static aabb getAabb(curTrans,temporalAabbMin,temporalAabbMax); - float temporalAabbMaxx = temporalAabbMax.getX(); - float temporalAabbMaxy = temporalAabbMax.getY(); - float temporalAabbMaxz = temporalAabbMax.getZ(); - float temporalAabbMinx = temporalAabbMin.getX(); - float temporalAabbMiny = temporalAabbMin.getY(); - float temporalAabbMinz = temporalAabbMin.getZ(); + btScalar temporalAabbMaxx = temporalAabbMax.getX(); + btScalar temporalAabbMaxy = temporalAabbMax.getY(); + btScalar temporalAabbMaxz = temporalAabbMax.getZ(); + btScalar temporalAabbMinx = temporalAabbMin.getX(); + btScalar temporalAabbMiny = temporalAabbMin.getY(); + btScalar temporalAabbMinz = temporalAabbMin.getZ(); // add linear motion btVector3 linMotion = linvel*timeStep; //todo: simd would have a vector max/min operation, instead of per-element access - if (linMotion.x() > 0.f) + if (linMotion.x() > btScalar(0.)) temporalAabbMaxx += linMotion.x(); else temporalAabbMinx += linMotion.x(); - if (linMotion.y() > 0.f) + if (linMotion.y() > btScalar(0.)) temporalAabbMaxy += linMotion.y(); else temporalAabbMiny += linMotion.y(); - if (linMotion.z() > 0.f) + if (linMotion.z() > btScalar(0.)) temporalAabbMaxz += linMotion.z(); else temporalAabbMinz += linMotion.z(); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index 7b2a00a1c57..96268734a83 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -16,18 +16,18 @@ subject to the following restrictions: #ifndef COLLISION_SHAPE_H #define COLLISION_SHAPE_H -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" -#include <LinearMath/btMatrix3x3.h> -#include "LinearMath/btPoint3.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btPoint3.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" //for the shape types ///btCollisionShape provides interface for collision shapes that can be shared among btCollisionObjects. class btCollisionShape { public: - btCollisionShape() :m_tempDebug(0) + btCollisionShape() { } virtual ~btCollisionShape() @@ -40,14 +40,15 @@ public: virtual void getBoundingSphere(btVector3& center,btScalar& radius) const; ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. - virtual float getAngularMotionDisc() const; + virtual btScalar getAngularMotionDisc() const; - virtual int getShapeType() const=0; ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax); +#ifndef __SPU__ + inline bool isPolyhedral() const { return btBroadphaseProxy::isPolyhedral(getShapeType()); @@ -72,20 +73,20 @@ public: return btBroadphaseProxy::isInfinite(getShapeType()); } + virtual int getShapeType() const=0; virtual void setLocalScaling(const btVector3& scaling) =0; virtual const btVector3& getLocalScaling() const =0; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) = 0; + //debugging support virtual char* getName()const =0 ; - const char* getExtraDebugInfo() const { return m_tempDebug;} - void setExtraDebugInfo(const char* extraDebugInfo) { m_tempDebug = extraDebugInfo;} - const char * m_tempDebug; -//endif debugging support +#endif //__SPU__ + + - virtual void setMargin(float margin) = 0; - virtual float getMargin() const = 0; + virtual void setMargin(btScalar margin) = 0; + virtual btScalar getMargin() const = 0; }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 88ae8c7dfd4..a4712b3e925 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -20,11 +20,11 @@ subject to the following restrictions: btCompoundShape::btCompoundShape() -:m_localAabbMin(1e30f,1e30f,1e30f), -m_localAabbMax(-1e30f,-1e30f,-1e30f), +:m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), +m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)), m_aabbTree(0), -m_collisionMargin(0.f), -m_localScaling(1.f,1.f,1.f) +m_collisionMargin(btScalar(0.)), +m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) { } @@ -60,8 +60,8 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { - btVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); btMatrix3x3 abs_b = trans.getBasis().absolute(); @@ -84,15 +84,15 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) btVector3 aabbMin,aabbMax; getAabb(ident,aabbMin,aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - btScalar lx=2.f*(halfExtents.x()); - btScalar ly=2.f*(halfExtents.y()); - btScalar lz=2.f*(halfExtents.z()); + btScalar lx=btScalar(2.)*(halfExtents.x()); + btScalar ly=btScalar(2.)*(halfExtents.y()); + btScalar lz=btScalar(2.)*(halfExtents.z()); - inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); - inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); - inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); + inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); + inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 84188bc8b76..2d384bd2180 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -18,12 +18,11 @@ subject to the following restrictions: #include "btCollisionShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include <vector> -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btCollisionMargin.h" +#include "../../LinearMath/btAlignedObjectArray.h" class btOptimizedBvh; @@ -85,11 +84,11 @@ public: virtual int getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} - virtual void setMargin(float margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual float getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp index 7cd35a91860..5103500a012 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "btConcaveShape.h" -btConcaveShape::btConcaveShape() : m_collisionMargin(0.f) +btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.)) { } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index 2bbba88bc44..73f974e4ee9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef CONCAVE_SHAPE_H #define CONCAVE_SHAPE_H -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - +#include "btCollisionShape.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "btTriangleCallback.h" @@ -27,7 +26,7 @@ subject to the following restrictions: class btConcaveShape : public btCollisionShape { protected: - float m_collisionMargin; + btScalar m_collisionMargin; public: btConcaveShape(); @@ -36,10 +35,10 @@ public: virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0; - virtual float getMargin() const { + virtual btScalar getMargin() const { return m_collisionMargin; } - virtual void setMargin(float collisionMargin) + virtual void setMargin(btScalar collisionMargin) { m_collisionMargin = collisionMargin; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 46232b3187c..207b3024bc3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -24,7 +24,7 @@ m_height(height) { setConeUpIndex(1); btVector3 halfExtents; - m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height)); + m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); } btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height): @@ -67,15 +67,15 @@ void btConeShape::setConeUpIndex(int upIndex) btVector3 btConeShape::coneLocalSupport(const btVector3& v) const { - float halfHeight = m_height * 0.5f; + btScalar halfHeight = m_height * btScalar(0.5); if (v[m_coneIndices[1]] > v.length() * m_sinAngle) { btVector3 tmp; - tmp[m_coneIndices[0]] = 0.f; + tmp[m_coneIndices[0]] = btScalar(0.); tmp[m_coneIndices[1]] = halfHeight; - tmp[m_coneIndices[2]] = 0.f; + tmp[m_coneIndices[2]] = btScalar(0.); return tmp; } else { @@ -90,9 +90,9 @@ btVector3 btConeShape::coneLocalSupport(const btVector3& v) const } else { btVector3 tmp; - tmp[m_coneIndices[0]] = 0.f; + tmp[m_coneIndices[0]] = btScalar(0.); tmp[m_coneIndices[1]] = -halfHeight; - tmp[m_coneIndices[2]] = 0.f; + tmp[m_coneIndices[2]] = btScalar(0.); return tmp; } } @@ -117,12 +117,12 @@ void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVect btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = coneLocalSupport(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 52d925fe714..3ccda5b12c6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -17,16 +17,16 @@ subject to the following restrictions: #define CONE_MINKOWSKI_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types ///btConeShape implements a Cone shape, around the Y axis class btConeShape : public btConvexShape { - float m_sinAngle; - float m_radius; - float m_height; + btScalar m_sinAngle; + btScalar m_radius; + btScalar m_height; int m_coneIndices[3]; btVector3 coneLocalSupport(const btVector3& v) const; @@ -38,8 +38,8 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - float getRadius() const { return m_radius;} - float getHeight() const { return m_height;} + btScalar getRadius() const { return m_radius;} + btScalar getHeight() const { return m_height;} virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) @@ -49,17 +49,17 @@ public: btVector3 aabbMin,aabbMax; getAabb(identity,aabbMin,aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - float margin = getMargin(); + btScalar margin = getMargin(); - btScalar lx=2.f*(halfExtents.x()+margin); - btScalar ly=2.f*(halfExtents.y()+margin); - btScalar lz=2.f*(halfExtents.z()+margin); + btScalar lx=btScalar(2.)*(halfExtents.x()+margin); + btScalar ly=btScalar(2.)*(halfExtents.y()+margin); + btScalar lz=btScalar(2.)*(halfExtents.z()+margin); const btScalar x2 = lx*lx; const btScalar y2 = ly*ly; const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * 0.08333333f; + const btScalar scaledmass = mass * btScalar(0.08333333); inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 5af6e5f03d9..deb3954b5c3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: -btConvexHullShape ::btConvexHullShape (const float* points,int numPoints,int stride) +btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) { m_points.resize(numPoints); @@ -30,26 +30,37 @@ btConvexHullShape ::btConvexHullShape (const float* points,int numPoints,int str btPoint3* point = (btPoint3*)(pointsBaseAddress + i*stride); m_points[i] = point[0]; } + + recalcLocalAabb(); + +} + + +void btConvexHullShape::addPoint(const btPoint3& point) +{ + m_points.push_back(point); + recalcLocalAabb(); + } btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { - btVector3 supVec(0.f,0.f,0.f); - btScalar newDot,maxDot = -1e30f; + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < 0.0001f) + if (lenSqr < btScalar(0.0001)) { vec.setValue(1,0,0); } else { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); vec *= rlen; } - for (size_t i=0;i<m_points.size();i++) + for (int i=0;i<m_points.size();i++) { btPoint3 vtx = m_points[i] * m_localScaling; @@ -70,10 +81,10 @@ void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i][3] = -1e30f; + supportVerticesOut[i][3] = btScalar(-1e30); } } - for (size_t i=0;i<m_points.size();i++) + for (int i=0;i<m_points.size();i++) { btPoint3 vtx = m_points[i] * m_localScaling; @@ -101,12 +112,12 @@ btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; @@ -153,13 +164,14 @@ int btConvexHullShape::getNumPlanes() const return 0; } -void btConvexHullShape::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const +void btConvexHullShape::getPlane(btVector3& ,btPoint3& ,int ) const { - assert(0); + + btAssert(0); } //not yet -bool btConvexHullShape::isInside(const btPoint3& pt,btScalar tolerance) const +bool btConvexHullShape::isInside(const btPoint3& ,btScalar ) const { assert(0); return false; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index beea0e63201..3fd5e382525 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -17,29 +17,37 @@ subject to the following restrictions: #define CONVEX_HULL_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - - -#include "LinearMath/btAlignedObjectArray.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../../LinearMath/btAlignedObjectArray.h" ///ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) ///No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. ///on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash. ///(memory is much slower then the cpu) -class btConvexHullShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape { btAlignedObjectArray<btPoint3> m_points; public: - ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive float (x,y,z), the striding defines the number of bytes between each point, in memory. + + + ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory. ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. ///btConvexHullShape make an internal copy of the points. - btConvexHullShape(const float* points=0,int numPoints=0, int stride=sizeof(btPoint3)); + btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btPoint3)); + + void addPoint(const btPoint3& point); - void addPoint(const btPoint3& point) + btPoint3* getPoints() { - m_points.push_back(point); + return &m_points[0]; } + + int getNumPoints() + { + return m_points.size(); + } + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 9537235ff8a..7edf1ea6db8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -15,8 +15,9 @@ subject to the following restrictions: #include "btConvexShape.h" + btConvexShape::btConvexShape() -: m_localScaling(1.f,1.f,1.f), +: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), m_collisionMargin(CONVEX_DISTANCE_MARGIN) { } @@ -35,35 +36,42 @@ void btConvexShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVec btScalar margin = getMargin(); for (int i=0;i<3;i++) { - btVector3 vec(0.f,0.f,0.f); - vec[i] = 1.f; + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); btVector3 tmp = trans(sv); maxAabb[i] = tmp[i]+margin; - vec[i] = -1.f; + vec[i] = btScalar(-1.); tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); minAabb[i] = tmp[i]-margin; } }; + btVector3 btConvexShape::localGetSupportingVertex(const btVector3& vec)const - { +{ +#ifndef __SPU__ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; } return supVertex; +#else + return btVector3(0,0,0); +#endif //__SPU__ + } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 0de334b3521..746f383dfc7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -18,11 +18,10 @@ subject to the following restrictions: #include "btCollisionShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include <vector> -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btCollisionMargin.h" //todo: get rid of this btConvexCastResult thing! struct btConvexCastResult; @@ -31,9 +30,9 @@ struct btConvexCastResult; /// btConvexShape is an abstract shape interface. /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. /// used in combination with GJK or btConvexCast -class btConvexShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape { - + protected: //local scaling. collisionMargin is not scaled ! @@ -43,14 +42,27 @@ protected: btScalar m_collisionMargin; + btScalar m_padding[2]; + + + + public: btConvexShape(); + virtual ~btConvexShape() + { + + } + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; +#ifndef __SPU__ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; +#endif //#ifndef __SPU__ const btVector3& getImplicitShapeDimensions() const { @@ -74,12 +86,21 @@ public: return m_localScaling; } + const btVector3& getLocalScalingNV() const + { + return m_localScaling; + } - virtual void setMargin(float margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual float getMargin() const + virtual btScalar getMargin() const + { + return m_collisionMargin; + } + + btScalar getMarginNV() const { return m_collisionMargin; } @@ -91,12 +112,15 @@ public: virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - assert(0); + (void)penetrationVector; + (void)index; + btAssert(0); } -}; +} +; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 9d8e7e1408e..614ec977793 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface) :m_stridingMesh(meshInterface) { + recalcLocalAabb(); } @@ -39,14 +40,17 @@ public: btVector3 m_supportVecLocal; LocalSupportVertexCallback(const btVector3& supportVecLocal) - : m_supportVertexLocal(0.f,0.f,0.f), - m_maxDot(-1e30f), + : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), + m_maxDot(btScalar(-1e30)), m_supportVecLocal(supportVecLocal) { } virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) { + (void)triangleIndex; + (void)partId; + for (int i=0;i<3;i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); @@ -71,21 +75,21 @@ public: btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { - btVector3 supVec(0.f,0.f,0.f); + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < 0.0001f) + if (lenSqr < btScalar(0.0001)) { vec.setValue(1,0,0); } else { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); vec *= rlen; } LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(1e30f,1e30f,1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); supVec = supportCallback.GetSupportVertexLocal(); @@ -98,7 +102,7 @@ void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargi { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i][3] = -1e30f; + supportVerticesOut[i][3] = btScalar(-1e30); } } @@ -109,7 +113,7 @@ void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargi { const btVector3& vec = vectors[j]; LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(1e30f,1e30f,1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); } @@ -122,12 +126,12 @@ btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& v { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; @@ -157,14 +161,14 @@ int btConvexTriangleMeshShape::getNumEdges() const return 0; } -void btConvexTriangleMeshShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const +void btConvexTriangleMeshShape::getEdge(int ,btPoint3& ,btPoint3& ) const { - assert(0); + btAssert(0); } -void btConvexTriangleMeshShape::getVertex(int i,btPoint3& vtx) const +void btConvexTriangleMeshShape::getVertex(int ,btPoint3& ) const { - assert(0); + btAssert(0); } int btConvexTriangleMeshShape::getNumPlanes() const @@ -172,15 +176,15 @@ int btConvexTriangleMeshShape::getNumPlanes() const return 0; } -void btConvexTriangleMeshShape::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const +void btConvexTriangleMeshShape::getPlane(btVector3& ,btPoint3& ,int ) const { - assert(0); + btAssert(0); } //not yet -bool btConvexTriangleMeshShape::isInside(const btPoint3& pt,btScalar tolerance) const +bool btConvexTriangleMeshShape::isInside(const btPoint3& ,btScalar ) const { - assert(0); + btAssert(0); return false; } @@ -189,5 +193,13 @@ bool btConvexTriangleMeshShape::isInside(const btPoint3& pt,btScalar tolerance) void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_stridingMesh->setScaling(scaling); + + recalcLocalAabb(); + } + +const btVector3& btConvexTriangleMeshShape::getLocalScaling() const +{ + return m_stridingMesh->getScaling(); +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index 86e871c6c0b..34ee7af744c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -3,9 +3,8 @@ #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include <vector> /// btConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use btConvexHullShape instead. /// It uses the btStridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. @@ -40,7 +39,8 @@ public: virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; - void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 16b263474f0..1666afb3b88 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -16,23 +16,33 @@ subject to the following restrictions: #include "LinearMath/btPoint3.h" btCylinderShape::btCylinderShape (const btVector3& halfExtents) -:btBoxShape(halfExtents) +:btBoxShape(halfExtents), +m_upAxis(1) { - + recalcLocalAabb(); } btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) :btCylinderShape(halfExtents) { + m_upAxis = 0; + recalcLocalAabb(); } btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) :btCylinderShape(halfExtents) { + m_upAxis = 2; + recalcLocalAabb(); } +void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + //skip the box 'getAabb' + btPolyhedralConvexShape::getAabb(t,aabbMin,aabbMax); +} inline btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) @@ -46,8 +56,8 @@ const int ZZ = 2; // extents of the cylinder is: X,Y is for radius, and Z for height - float radius = halfExtents[XX]; - float halfHeight = halfExtents[cylinderUpAxis]; + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; btVector3 tmp; @@ -87,8 +97,8 @@ const int YY = 1; const int ZZ = 2; - float radius = halfExtents[XX]; - float halfHeight = halfExtents[cylinderUpAxis]; + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; btVector3 tmp; @@ -124,8 +134,8 @@ const int ZZ = 1; // extents of the cylinder is: X,Y is for radius, and Z for height - float radius = halfExtents[XX]; - float halfHeight = halfExtents[cylinderUpAxis]; + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; btVector3 tmp; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h index bb0e4298abc..c78890c887a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -17,22 +17,23 @@ subject to the following restrictions: #define CYLINDER_MINKOWSKI_H #include "btBoxShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include "LinearMath/btVector3.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../../LinearMath/btVector3.h" /// implements cylinder shape interface class btCylinderShape : public btBoxShape { +protected: + + int m_upAxis; + public: btCylinderShape (const btVector3& halfExtents); ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - getAabbSlow(t,aabbMin,aabbMax); - } + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; @@ -44,12 +45,12 @@ public: btVector3 supVertex; supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; @@ -66,12 +67,12 @@ public: return CYLINDER_SHAPE_PROXYTYPE; } - virtual int getUpAxis() const + int getUpAxis() const { - return 1; + return m_upAxis; } - virtual float getRadius() const + virtual btScalar getRadius() const { return getHalfExtents().getX(); } @@ -93,17 +94,14 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getUpAxis() const - { - return 0; - } + //debugging virtual char* getName()const { return "CylinderX"; } - virtual float getRadius() const + virtual btScalar getRadius() const { return getHalfExtents().getY(); } @@ -128,7 +126,7 @@ public: return "CylinderZ"; } - virtual float getRadius() const + virtual btScalar getRadius() const { return getHalfExtents().getX(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index c63f4a9c571..dcc486f194b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -40,9 +40,9 @@ void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aa } -void btEmptyShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) { - assert(0); + btAssert(0); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h index c16149e55fb..8ec53444976 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -18,11 +18,10 @@ subject to the following restrictions: #include "btConcaveShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include <vector> -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btCollisionMargin.h" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 57132ee9512..5a359c0cff1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: btHeightfieldTerrainShape::btHeightfieldTerrainShape() -:m_localScaling(0.f,0.f,0.f) +:m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.)) { } @@ -30,11 +30,10 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape() -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btHeightfieldTerrainShape::getAabb(const btTransform& ,btVector3& aabbMin,btVector3& aabbMax) const { - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); - + aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); } @@ -42,14 +41,17 @@ void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin, void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { + (void)callback; + (void)aabbMax; + (void)aabbMin; - btVector3 halfExtents = (aabbMax - aabbMin) * 0.5f; - btScalar radius = halfExtents.length(); - btVector3 center = (aabbMax + aabbMin) * 0.5f; + /* + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); + btVector3 center = (aabbMax + aabbMin) * btScalar(0.5); //TODO //this is where the triangles are generated, given AABB and plane equation (normal/constant) -/* + btVector3 tangentDir0,tangentDir1; //tangentDir0/tangentDir1 can be precalculated @@ -60,6 +62,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; btVector3 triangle[3]; + btScalar radius = halfExtents.length(); triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius; triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius; triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius; @@ -75,11 +78,11 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) { //moving concave objects not supported - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index e4cc9d42d59..85fd49638ce 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef HEIGHTFIELD_TERRAIN_SHAPE_H #define HEIGHTFIELD_TERRAIN_SHAPE_H -#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "btConcaveShape.h" ///btHeightfieldTerrainShape simulates a 2D heightfield terrain class btHeightfieldTerrainShape : public btConcaveShape diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 4bf8c478a53..b213f3819a4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -43,7 +43,7 @@ void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(cons -float btMinkowskiSumShape::getMargin() const +btScalar btMinkowskiSumShape::getMargin() const { return m_shapeA->getMargin() + m_shapeB->getMargin(); } @@ -51,6 +51,7 @@ float btMinkowskiSumShape::getMargin() const void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { - assert(0); + (void)mass; + btAssert(0); inertia.setValue(0,0,0); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index e974f57a4c2..489b7ddde1e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define MINKOWSKI_SUM_SHAPE_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types /// btMinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. class btMinkowskiSumShape : public btConvexShape @@ -48,7 +48,7 @@ public: virtual int getShapeType() const { return MINKOWSKI_SUM_SHAPE_PROXYTYPE; } - virtual float getMargin() const; + virtual btScalar getMargin() const; const btConvexShape* getShapeA() const { return m_shapeA;} const btConvexShape* getShapeB() const { return m_shapeB;} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index fde4d95da14..6015a618082 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres) :m_inertiaHalfExtents(inertiaHalfExtents) { - float startMargin = 1e30f; + btScalar startMargin = btScalar(1e30); m_numSpheres = numSpheres; for (int i=0;i<m_numSpheres;i++) @@ -42,17 +42,17 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons int i; btVector3 supVec(0,0,0); - btScalar maxDot(-1e30f); + btScalar maxDot(btScalar(-1e30)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < 0.0001f) + if (lenSqr < btScalar(0.0001)) { vec.setValue(1,0,0); } else { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); vec *= rlen; } @@ -84,7 +84,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons for (int j=0;j<numVectors;j++) { - btScalar maxDot(-1e30f); + btScalar maxDot(btScalar(-1e30)); const btVector3& vec = vectors[j]; @@ -126,17 +126,17 @@ void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) // getAabb(ident,aabbMin,aabbMax); - btVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* 0.5f; + btVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* btScalar(0.5); - float margin = CONVEX_DISTANCE_MARGIN; + btScalar margin = CONVEX_DISTANCE_MARGIN; - btScalar lx=2.f*(halfExtents[0]+margin); - btScalar ly=2.f*(halfExtents[1]+margin); - btScalar lz=2.f*(halfExtents[2]+margin); + btScalar lx=btScalar(2.)*(halfExtents[0]+margin); + btScalar ly=btScalar(2.)*(halfExtents[1]+margin); + btScalar lz=btScalar(2.)*(halfExtents[2]+margin); const btScalar x2 = lx*lx; const btScalar y2 = ly*ly; const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * 0.08333333f; + const btScalar scaledmass = mass * btScalar(.08333333); inertia[0] = scaledmass * (y2+z2); inertia[1] = scaledmass * (x2+z2); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 2dde9d3855a..be044dbc16a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define MULTI_SPHERE_MINKOWSKI_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types #define MAX_NUM_SPHERES 5 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 37f15e1dcc4..44438a24455 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -16,89 +16,373 @@ subject to the following restrictions: #include "btOptimizedBvh.h" #include "btStridingMeshInterface.h" #include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btIDebugDraw.h" -btOptimizedBvh::btOptimizedBvh() :m_rootNode1(0), m_numNodes(0) + +btOptimizedBvh::btOptimizedBvh() : m_useQuantization(false), + m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) + //m_traversalMode(TRAVERSAL_STACKLESS) + //m_traversalMode(TRAVERSAL_RECURSIVE) { } -void btOptimizedBvh::build(btStridingMeshInterface* triangles) +void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax) { - //int countTriangles = 0; + m_useQuantization = useQuantizedAabbCompression; - // NodeArray triangleNodes; struct NodeTriangleCallback : public btInternalTriangleIndexCallback { + NodeArray& m_triangleNodes; + NodeTriangleCallback& operator=(NodeTriangleCallback& other) + { + m_triangleNodes = other.m_triangleNodes; + return *this; + } + NodeTriangleCallback(NodeArray& triangleNodes) :m_triangleNodes(triangleNodes) { - } virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) { - btOptimizedBvhNode node; - node.m_aabbMin = btVector3(1e30f,1e30f,1e30f); - node.m_aabbMax = btVector3(-1e30f,-1e30f,-1e30f); - node.m_aabbMin.setMin(triangle[0]); - node.m_aabbMax.setMax(triangle[0]); - node.m_aabbMin.setMin(triangle[1]); - node.m_aabbMax.setMax(triangle[1]); - node.m_aabbMin.setMin(triangle[2]); - node.m_aabbMax.setMax(triangle[2]); + btVector3 aabbMin,aabbMax; + aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setMin(triangle[0]); + aabbMax.setMax(triangle[0]); + aabbMin.setMin(triangle[1]); + aabbMax.setMax(triangle[1]); + aabbMin.setMin(triangle[2]); + aabbMax.setMax(triangle[2]); + + //with quantization? + node.m_aabbMinOrg = aabbMin; + node.m_aabbMaxOrg = aabbMax; node.m_escapeIndex = -1; - node.m_leftChild = 0; - node.m_rightChild = 0; - - + //for child nodes node.m_subPart = partId; node.m_triangleIndex = triangleIndex; + m_triangleNodes.push_back(node); + } + }; + struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback + { + QuantizedNodeArray& m_triangleNodes; + const btOptimizedBvh* m_optimizedTree; // for quantization + + QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) + { + m_triangleNodes = other.m_triangleNodes; + m_optimizedTree = other.m_optimizedTree; + return *this; + } + + QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btOptimizedBvh* tree) + :m_triangleNodes(triangleNodes),m_optimizedTree(tree) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + btAssert(partId==0); + //negative indices are reserved for escapeIndex + btAssert(triangleIndex>=0); + + btQuantizedBvhNode node; + btVector3 aabbMin,aabbMax; + aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setMin(triangle[0]); + aabbMax.setMax(triangle[0]); + aabbMin.setMin(triangle[1]); + aabbMax.setMax(triangle[1]); + aabbMin.setMin(triangle[2]); + aabbMax.setMax(triangle[2]); + + m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMin[0],aabbMin); + m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax); + + node.m_escapeIndexOrTriangleIndex = triangleIndex; - m_triangleNodes.push_back(node); } }; + + + + int numLeafNodes = 0; + if (m_useQuantization) + { - NodeTriangleCallback callback(m_leafNodes); + //initialize quantization values + setQuantizationValues(bvhAabbMin,bvhAabbMax); + + QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); + + + triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); - btVector3 aabbMin(-1e30f,-1e30f,-1e30f); - btVector3 aabbMax(1e30f,1e30f,1e30f); + //now we have an array of leafnodes in m_leafNodes + numLeafNodes = m_quantizedLeafNodes.size(); - triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); - //now we have an array of leafnodes in m_leafNodes + m_quantizedContiguousNodes.resize(2*numLeafNodes); + + + } else + { + NodeTriangleCallback callback(m_leafNodes); + + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + + triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + + //now we have an array of leafnodes in m_leafNodes + numLeafNodes = m_leafNodes.size(); + + m_contiguousNodes.resize(2*numLeafNodes); + } - m_contiguousNodes = new btOptimizedBvhNode[2*m_leafNodes.size()]; m_curNodeIndex = 0; - m_rootNode1 = buildTree(m_leafNodes,0,m_leafNodes.size()); + buildTree(0,numLeafNodes); + + ///if the entire tree is small then subtree size, we need to create a header info for the tree + if(m_useQuantization && !m_SubtreeHeaders.size()) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); + subtree.m_rootNodeIndex = 0; + subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); + } +} + + + +void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +{ + //incrementally initialize quantization values + btAssert(m_useQuantization); + + btAssert(aabbMin.getX() > m_bvhAabbMin.getX()); + btAssert(aabbMin.getY() > m_bvhAabbMin.getY()); + btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ()); + + btAssert(aabbMax.getX() < m_bvhAabbMax.getX()); + btAssert(aabbMax.getY() < m_bvhAabbMax.getY()); + btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ()); + + ///we should update all quantization values, using updateBvhNodes(meshInterface); + ///but we only update chunks that overlap the given aabb + + unsigned short quantizedQueryAabbMin[3]; + unsigned short quantizedQueryAabbMax[3]; + + quantizeWithClamp(&quantizedQueryAabbMin[0],aabbMin); + quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax); + int i; + for (i=0;i<this->m_SubtreeHeaders.size();i++) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - ///create the leafnodes first -// btOptimizedBvhNode* leafNodes = new btOptimizedBvhNode; + bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap) + { + updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); + + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); + } + } + } +///just for debugging, to visualize the individual patches/subtrees +#ifdef DEBUG_PATCH_COLORS +btVector3 color[4]= +{ + btVector3(255,0,0), + btVector3(0,255,0), + btVector3(0,0,255), + btVector3(0,255,255) +}; +#endif //DEBUG_PATCH_COLORS + + +void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) +{ + (void)index; + + btAssert(m_useQuantization); + + int nodeSubPart=0; + + //get access info to trianglemesh data + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); + + btVector3 triangleVerts[3]; + btVector3 aabbMin,aabbMax; + const btVector3& meshScaling = meshInterface->getScaling(); + + int i; + for (i=endNode-1;i>=firstNode;i--) + { + + + btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; + if (curNode.isLeafNode()) + { + //recalc aabb from triangle data + int nodeTriangleIndex = curNode.getTriangleIndex(); + //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, + + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + + + for (int j=2;j>=0;j--) + { + + int graphicsindex = gfxbase[j]; + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); +#ifdef DEBUG_PATCH_COLORS + btVector3 mycolor = color[index&3]; + graphicsbase[8] = mycolor.getX(); + graphicsbase[9] = mycolor.getY(); + graphicsbase[10] = mycolor.getZ(); +#endif //DEBUG_PATCH_COLORS + + + triangleVerts[j] = btVector3( + graphicsbase[0]*meshScaling.getX(), + graphicsbase[1]*meshScaling.getY(), + graphicsbase[2]*meshScaling.getZ()); + } + + + + aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setMin(triangleVerts[0]); + aabbMax.setMax(triangleVerts[0]); + aabbMin.setMin(triangleVerts[1]); + aabbMax.setMax(triangleVerts[1]); + aabbMin.setMin(triangleVerts[2]); + aabbMax.setMax(triangleVerts[2]); + + quantizeWithClamp(&curNode.m_quantizedAabbMin[0],aabbMin); + quantizeWithClamp(&curNode.m_quantizedAabbMax[0],aabbMax); + + } else + { + //combine aabb from both children + + btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; + + btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : + &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; + + + { + for (int i=0;i<3;i++) + { + curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; + if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i]) + curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i]; + + curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; + if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) + curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; + } + } + } + + } + + meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + + +} + +void btOptimizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin) +{ + //enlarge the AABB to avoid division by zero when initializing the quantization values + btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + m_bvhAabbMin = bvhAabbMin - clampValue; + m_bvhAabbMax = bvhAabbMax + clampValue; + btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = btVector3(btScalar(65535.0),btScalar(65535.0),btScalar(65535.0)) / aabbSize; +} + + +void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface) +{ + if (m_useQuantization) + { + //calculate new aabb + btVector3 aabbMin,aabbMax; + meshInterface->calculateAabbBruteForce(aabbMin,aabbMax); + + setQuantizationValues(aabbMin,aabbMax); + + updateBvhNodes(meshInterface,0,m_curNodeIndex,0); + + ///now update all subtree headers + + int i; + for (i=0;i<m_SubtreeHeaders.size();i++) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); + } + + } else + { + + } +} + + + btOptimizedBvh::~btOptimizedBvh() { - if (m_contiguousNodes) - delete []m_contiguousNodes; } -btOptimizedBvhNode* btOptimizedBvh::buildTree (NodeArray& leafNodes,int startIndex,int endIndex) +#ifdef DEBUG_TREE_BUILDING +int gStackDepth = 0; +int gMaxStackDepth = 0; +#endif //DEBUG_TREE_BUILDING + +void btOptimizedBvh::buildTree (int startIndex,int endIndex) { - btOptimizedBvhNode* internalNode; +#ifdef DEBUG_TREE_BUILDING + gStackDepth++; + if (gStackDepth > gMaxStackDepth) + gMaxStackDepth = gStackDepth; +#endif //DEBUG_TREE_BUILDING + int splitAxis, splitIndex, i; int numIndices =endIndex-startIndex; @@ -108,96 +392,172 @@ btOptimizedBvhNode* btOptimizedBvh::buildTree (NodeArray& leafNodes,int startInd if (numIndices==1) { - return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); + + m_curNodeIndex++; + return; } //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. - splitAxis = calcSplittingAxis(leafNodes,startIndex,endIndex); + splitAxis = calcSplittingAxis(startIndex,endIndex); - splitIndex = sortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis); + splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); - internalNode = &m_contiguousNodes[m_curNodeIndex++]; + int internalNodeIndex = m_curNodeIndex; - internalNode->m_aabbMax.setValue(-1e30f,-1e30f,-1e30f); - internalNode->m_aabbMin.setValue(1e30f,1e30f,1e30f); + setInternalNodeAabbMax(m_curNodeIndex,btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30))); + setInternalNodeAabbMin(m_curNodeIndex,btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30))); for (i=startIndex;i<endIndex;i++) { - internalNode->m_aabbMax.setMax(leafNodes[i].m_aabbMax); - internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin); + mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i)); } + m_curNodeIndex++; //internalNode->m_escapeIndex; - internalNode->m_leftChild = buildTree(leafNodes,startIndex,splitIndex); - internalNode->m_rightChild = buildTree(leafNodes,splitIndex,endIndex); + + int leftChildNodexIndex = m_curNodeIndex; + + //build left child tree + buildTree(startIndex,splitIndex); + + int rightChildNodexIndex = m_curNodeIndex; + //build right child tree + buildTree(splitIndex,endIndex); + +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + int escapeIndex = m_curNodeIndex - curIndex; + + if (m_useQuantization) + { + //escapeIndex is the number of nodes of this subtree + const int sizeQuantizedNode =sizeof(btQuantizedBvhNode); + const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; + if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) + { + updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); + } + } + + setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); - internalNode->m_escapeIndex = m_curNodeIndex - curIndex; - return internalNode; } -int btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis) +void btOptimizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +{ + btAssert(m_useQuantization); + + btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; + int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); + int leftSubTreeSizeInBytes = leftSubTreeSize * sizeof(btQuantizedBvhNode); + + btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; + int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); + int rightSubTreeSizeInBytes = rightSubTreeSize * sizeof(btQuantizedBvhNode); + + if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(leftChildNode); + subtree.m_rootNodeIndex = leftChildNodexIndex; + subtree.m_subtreeSize = leftSubTreeSize; + } + + if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(rightChildNode); + subtree.m_rootNodeIndex = rightChildNodexIndex; + subtree.m_subtreeSize = rightSubTreeSize; + } +} + + +int btOptimizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) { int i; int splitIndex =startIndex; int numIndices = endIndex - startIndex; - float splitValue; + btScalar splitValue; - btVector3 means(0.f,0.f,0.f); + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); for (i=startIndex;i<endIndex;i++) { - btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); means+=center; } - means *= (1.f/(float)numIndices); + means *= (btScalar(1.)/(btScalar)numIndices); splitValue = means[splitAxis]; //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. for (i=startIndex;i<endIndex;i++) { - btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); if (center[splitAxis] > splitValue) { //swap - btOptimizedBvhNode tmp = leafNodes[i]; - leafNodes[i] = leafNodes[splitIndex]; - leafNodes[splitIndex] = tmp; + swapLeafNodes(i,splitIndex); splitIndex++; } } - if ((splitIndex==startIndex) || (splitIndex == (endIndex-1))) + + //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + //otherwise the tree-building might fail due to stack-overflows in certain cases. + //unbalanced1 is unsafe: it can cause stack overflows + //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + //unbalanced2 should work too: always use center (perfect balanced trees) + //bool unbalanced2 = true; + + //this should be safe too: + int rangeBalancedIndices = numIndices/3; + bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + + if (unbalanced) { splitIndex = startIndex+ (numIndices>>1); } + + bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + btAssert(!unbal); + return splitIndex; } -int btOptimizedBvh::calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex) +int btOptimizedBvh::calcSplittingAxis(int startIndex,int endIndex) { int i; - btVector3 means(0.f,0.f,0.f); - btVector3 variance(0.f,0.f,0.f); + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); int numIndices = endIndex-startIndex; for (i=startIndex;i<endIndex;i++) { - btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); means+=center; } - means *= (1.f/(float)numIndices); + means *= (btScalar(1.)/(btScalar)numIndices); for (i=startIndex;i<endIndex;i++) { - btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); btVector3 diff2 = center-means; diff2 = diff2 * diff2; variance += diff2; } - variance *= (1.f/ ((float)numIndices-1) ); + variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); return variance.maxAxis(); } @@ -208,11 +568,83 @@ void btOptimizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb { //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) - //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); - walkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); + if (m_useQuantization) + { + ///quantize query AABB + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,aabbMin); + quantizeWithClamp(quantizedQueryAabbMax,aabbMax); + + switch (m_traversalMode) + { + case TRAVERSAL_STACKLESS: + walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,0,m_curNodeIndex); + break; + case TRAVERSAL_STACKLESS_CACHE_FRIENDLY: + walkStacklessQuantizedTreeCacheFriendly(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + break; + case TRAVERSAL_RECURSIVE: + { + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0]; + walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + } + break; + default: + //unsupported + btAssert(0); + } + } else + { + walkStacklessTree(nodeCallback,aabbMin,aabbMax); + } } + +int maxIterations = 0; + +void btOptimizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + btAssert(!m_useQuantization); + + const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0]; + int escapeIndex, curIndex = 0; + int walkIterations = 0; + bool aabbOverlap, isLeafNode; + + while (curIndex < m_curNodeIndex) + { + //catch bugs in tree data + assert (walkIterations < m_curNodeIndex); + + walkIterations++; + aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); + isLeafNode = rootNode->m_escapeIndex == -1; + + if (isLeafNode && aabbOverlap) + { + nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); + } + + if (aabbOverlap || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +/* +///this was the original recursive traversal, before we optimized towards stackless traversal void btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const { bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); @@ -230,27 +662,82 @@ void btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback } } +*/ -int maxIterations = 0; +void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + bool aabbOverlap, isLeafNode; -void btOptimizedBvh::walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); + isLeafNode = currentNode->isLeafNode(); + + if (aabbOverlap) + { + if (isLeafNode) + { + nodeCallback->processNode(0,currentNode->getTriangleIndex()); + } else + { + //process left and right children + const btQuantizedBvhNode* leftChildNode = currentNode+1; + walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + + const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); + walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + } + } +} + + + + + + + +void btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const { - int escapeIndex, curIndex = 0; + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + bool aabbOverlap, isLeafNode; - while (curIndex < m_curNodeIndex) + while (curIndex < endNodeIndex) { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + //catch bugs in tree data - assert (walkIterations < m_curNodeIndex); + assert (walkIterations < subTreeSize); walkIterations++; - aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); - isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); if (isLeafNode && aabbOverlap) { - nodeCallback->processNode(rootNode); + nodeCallback->processNode(0,rootNode->getTriangleIndex()); } if (aabbOverlap || isLeafNode) @@ -259,21 +746,100 @@ void btOptimizedBvh::walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverla curIndex++; } else { - escapeIndex = rootNode->m_escapeIndex; + escapeIndex = rootNode->getEscapeIndex(); rootNode += escapeIndex; curIndex += escapeIndex; } - } - if (maxIterations < walkIterations) maxIterations = walkIterations; } +//This traversal can be called from Playstation 3 SPU +void btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + int i; + + + for (i=0;i<this->m_SubtreeHeaders.size();i++) + { + const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + + bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap) + { + walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, + subtree.m_rootNodeIndex, + subtree.m_rootNodeIndex+subtree.m_subtreeSize); + } + } +} + + + void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const { + (void)nodeCallback; + (void)aabbMin; + (void)aabbMax; + //not yet, please use aabb + btAssert(0); +} + + +void btOptimizedBvh::quantizeWithClamp(unsigned short* out, const btVector3& point) const +{ + + btAssert(m_useQuantization); + + btVector3 clampedPoint(point); + clampedPoint.setMax(m_bvhAabbMin); + clampedPoint.setMin(m_bvhAabbMax); + + btVector3 v = (clampedPoint - m_bvhAabbMin) * m_bvhQuantization; + out[0] = (unsigned short)(v.getX()+0.5f); + out[1] = (unsigned short)(v.getY()+0.5f); + out[2] = (unsigned short)(v.getZ()+0.5f); +} + +btVector3 btOptimizedBvh::unQuantize(const unsigned short* vecIn) const +{ + btVector3 vecOut; + vecOut.setValue( + (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()), + (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()), + (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ())); + vecOut += m_bvhAabbMin; + return vecOut; +} + +void btOptimizedBvh::swapLeafNodes(int i,int splitIndex) +{ + if (m_useQuantization) + { + btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; + m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; + m_quantizedLeafNodes[splitIndex] = tmp; + } else + { + btOptimizedBvhNode tmp = m_leafNodes[i]; + m_leafNodes[i] = m_leafNodes[splitIndex]; + m_leafNodes[splitIndex] = tmp; + } } +void btOptimizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +{ + if (m_useQuantization) + { + m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; + } else + { + m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index cb76cb23340..d5159586344 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -17,103 +17,313 @@ subject to the following restrictions: #define OPTIMIZED_BVH_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp -#include <vector> - class btStridingMeshInterface; -/// btOptimizedBvhNode contains both internal and leaf node information. -/// It hasn't been optimized yet for storage. Some obvious optimizations are: -/// Removal of the pointers (can already be done, they are not used for traversal) -/// and storing aabbmin/max as quantized integers. -/// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle -/// meshes stored in a non-uniform way (like batches/subparts of triangle-fans -ATTRIBUTE_ALIGNED16 (struct btOptimizedBvhNode) -{ +//Note: currently we have 16 bytes per quantized node +#define MAX_SUBTREE_SIZE_IN_BYTES 2048 - btVector3 m_aabbMin; - btVector3 m_aabbMax; -//these 2 pointers are obsolete, the stackless traversal just uses the escape index - btOptimizedBvhNode* m_leftChild; - btOptimizedBvhNode* m_rightChild; +///btQuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode +{ + + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; + + bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrTriangleIndex >= 0); + } + int getEscapeIndex() const + { + btAssert(!isLeafNode()); + return -m_escapeIndexOrTriangleIndex; + } + int getTriangleIndex() const + { + btAssert(isLeafNode()); + return m_escapeIndexOrTriangleIndex; + } +} +; +/// btOptimizedBvhNode contains both internal and leaf node information. +/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. +ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode +{ + //32 bytes + btVector3 m_aabbMinOrg; + btVector3 m_aabbMaxOrg; + + //4 int m_escapeIndex; + //8 //for child nodes int m_subPart; int m_triangleIndex; + int m_padding[5];//bad, due to alignment + }; + +///btBvhSubtreeInfo provides info to gather a subtree of limited size +ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo +{ +public: + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; + + + void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) + { + m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; + m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; + m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; + m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; + m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; + m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; + } +} +; + + class btNodeOverlapCallback { public: virtual ~btNodeOverlapCallback() {}; - virtual void processNode(const btOptimizedBvhNode* node) = 0; + virtual void processNode(int subPart, int triangleIndex) = 0; }; -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btAlignedAllocator.h" +#include "../../LinearMath/btAlignedObjectArray.h" -//typedef std::vector< unsigned , allocator_type > container_type; -const unsigned size = (1 << 20); -typedef btAlignedAllocator< btOptimizedBvhNode , size > allocator_type; -//typedef btAlignedObjectArray<btOptimizedBvhNode, allocator_type> NodeArray; +///for code readability: typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray; +typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray; +typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray; ///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) -class btOptimizedBvh +ATTRIBUTE_ALIGNED16(class) btOptimizedBvh { NodeArray m_leafNodes; + NodeArray m_contiguousNodes; - btOptimizedBvhNode* m_rootNode1; + QuantizedNodeArray m_quantizedLeafNodes; + + QuantizedNodeArray m_quantizedContiguousNodes; - btOptimizedBvhNode* m_contiguousNodes; int m_curNodeIndex; - int m_numNodes; + //quantization data + bool m_useQuantization; + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; + enum btTraversalMode + { + TRAVERSAL_STACKLESS = 0, + TRAVERSAL_STACKLESS_CACHE_FRIENDLY, + TRAVERSAL_RECURSIVE + }; -public: - btOptimizedBvh(); + btTraversalMode m_traversalMode; - virtual ~btOptimizedBvh(); - void build(btStridingMeshInterface* triangles); - btOptimizedBvhNode* buildTree (NodeArray& leafNodes,int startIndex,int endIndex); - int calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex); + BvhSubtreeInfoArray m_SubtreeHeaders; + + + ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) + ///this might be refactored into a virtual, it is usually not calculated at run-time + void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) + { + if (m_useQuantization) + { + quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; + + } + } + void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) + { + if (m_useQuantization) + { + quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; + } + } + + btVector3 getAabbMin(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMinOrg; + + } + btVector3 getAabbMax(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMaxOrg; + + } + + void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); + + void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) + { + if (m_useQuantization) + { + m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; + } + else + { + m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; + } + + } + + void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) + { + if (m_useQuantization) + { + unsigned short int quantizedAabbMin[3]; + unsigned short int quantizedAabbMax[3]; + quantizeWithClamp(quantizedAabbMin,newAabbMin); + quantizeWithClamp(quantizedAabbMax,newAabbMax); + for (int i=0;i<3;i++) + { + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i]; + + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; + + } + } else + { + //non-quantized + m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); + m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); + } + } + + void swapLeafNodes(int firstIndex,int secondIndex); + + void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); + +protected: - int sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis); - void walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void buildTree (int startIndex,int endIndex); + + int calcSplittingAxis(int startIndex,int endIndex); + + int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); - void walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; + + ///tree traversal designed for small-memory processors like PS3 SPU + void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; - //OptimizedBvhNode* GetRootNode() { return m_rootNode1;} + inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } + + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + +public: + btOptimizedBvh(); + + virtual ~btOptimizedBvh(); - int getNumNodes() { return m_numNodes;} + void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; void reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void quantizeWithClamp(unsigned short* out, const btVector3& point) const; + + btVector3 unQuantize(const unsigned short* vecIn) const; -}; + ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees. + void setTraversalMode(btTraversalMode traversalMode) + { + m_traversalMode = traversalMode; + } + + void refit(btStridingMeshInterface* triangles); + + void refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax); + + void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index); + + + QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; + } + + BvhSubtreeInfoArray& getSubtreeInfoArray() + { + return m_SubtreeHeaders; + } + +} +; #endif //OPTIMIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 6f2272cc454..bbc4ba62af6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -16,7 +16,10 @@ subject to the following restrictions: #include <BulletCollision/CollisionShapes/btPolyhedralConvexShape.h> btPolyhedralConvexShape::btPolyhedralConvexShape() -:m_optionalHull(0) +:m_localAabbMin(1,1,1), +m_localAabbMax(-1,-1,-1), +m_isLocalAabbValid(false), +m_optionalHull(0) { } @@ -28,16 +31,16 @@ btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b int i; btVector3 supVec(0,0,0); - btScalar maxDot(-1e30f); + btScalar maxDot(btScalar(-1e30)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < 0.0001f) + if (lenSqr < btScalar(0.0001)) { vec.setValue(1,0,0); } else { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); vec *= rlen; } @@ -68,7 +71,7 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( for (i=0;i<numVectors;i++) { - supportVerticesOut[i][3] = -1e30f; + supportVerticesOut[i][3] = btScalar(-1e30); } for (int j=0;j<numVectors;j++) @@ -96,23 +99,50 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine { //not yet, return box inertia - float margin = getMargin(); + btScalar margin = getMargin(); btTransform ident; ident.setIdentity(); btVector3 aabbMin,aabbMax; getAabb(ident,aabbMin,aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - btScalar lx=2.f*(halfExtents.x()+margin); - btScalar ly=2.f*(halfExtents.y()+margin); - btScalar lz=2.f*(halfExtents.z()+margin); + btScalar lx=btScalar(2.)*(halfExtents.x()+margin); + btScalar ly=btScalar(2.)*(halfExtents.y()+margin); + btScalar lz=btScalar(2.)*(halfExtents.z()+margin); const btScalar x2 = lx*lx; const btScalar y2 = ly*ly; const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * 0.08333333f; + const btScalar scaledmass = mass * btScalar(0.08333333); inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); } + + +void btPolyhedralConvexShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); +} + + + + +void btPolyhedralConvexShape::recalcLocalAabb() +{ + m_isLocalAabbValid = true; + + for (int i=0;i<3;i++) + { + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); + btVector3 tmp = localGetSupportingVertex(vec); + m_localAabbMax[i] = tmp[i]+m_collisionMargin; + vec[i] = btScalar(-1.); + tmp = localGetSupportingVertex(vec); + m_localAabbMin[i] = tmp[i]-m_collisionMargin; + } +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index a404504ba86..c35f7512663 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -16,15 +16,20 @@ subject to the following restrictions: #ifndef BU_SHAPE #define BU_SHAPE -#include <LinearMath/btPoint3.h> -#include <LinearMath/btMatrix3x3.h> -#include <BulletCollision/CollisionShapes/btConvexShape.h> +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btConvexShape.h" ///PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes. class btPolyhedralConvexShape : public btConvexShape { +protected: + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + public: btPolyhedralConvexShape(); @@ -36,6 +41,39 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const + { + + //lazy evaluation of local aabb + btAssert(m_isLocalAabbValid); + + btAssert(m_localAabbMin.getX() <= m_localAabbMax.getX()); + btAssert(m_localAabbMin.getY() <= m_localAabbMax.getY()); + btAssert(m_localAabbMin.getZ() <= m_localAabbMax.getZ()); + + + btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btPoint3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += btVector3(margin,margin,margin); + + aabbMin = center - extent; + aabbMax = center + extent; + + + } + + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + void recalcLocalAabb(); virtual int getNumVertices() const = 0 ; virtual int getNumEdges() const = 0; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp index 39e458c0f22..ca65dd03f3e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -26,14 +26,17 @@ btSphereShape ::btSphereShape (btScalar radius) btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - return btVector3(0.f,0.f,0.f); + (void)vec; + return btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); } void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const { + (void)vectors; + for (int i=0;i<numVectors;i++) { - supportVerticesOut[i].setValue(0.f,0.f,0.f); + supportVerticesOut[i].setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } } @@ -46,7 +49,7 @@ btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec)const btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; @@ -67,8 +70,8 @@ void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& a void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { - btScalar elem = 0.4f * mass * getMargin()*getMargin(); - inertia[0] = inertia[1] = inertia[2] = elem; + btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin(); + inertia.setValue(elem,elem,elem); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h index 2db6a872b40..66521664087 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -17,10 +17,10 @@ subject to the following restrictions: #define SPHERE_MINKOWSKI_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types ///btSphereShape implements an implicit (getSupportingVertex) Sphere -class btSphereShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexShape { @@ -45,11 +45,11 @@ public: //debugging virtual char* getName()const {return "SPHERE";} - virtual void setMargin(float margin) + virtual void setMargin(btScalar margin) { btConvexShape::setMargin(margin); } - virtual float getMargin() const + virtual btScalar getMargin() const { //to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case //this means, non-uniform scaling is not supported anymore diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index 8c7022ba83b..4fbaafa1b93 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant) :m_planeNormal(planeNormal), m_planeConstant(planeConstant), -m_localScaling(0.f,0.f,0.f) +m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.)) { } @@ -34,16 +34,19 @@ btStaticPlaneShape::~btStaticPlaneShape() void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - btVector3 infvec (1e30f,1e30f,1e30f); + (void)t; + /* + btVector3 infvec (btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 center = m_planeNormal*m_planeConstant; aabbMin = center + infvec*m_planeNormal; aabbMax = aabbMin; aabbMin.setMin(center - infvec*m_planeNormal); aabbMax.setMax(center - infvec*m_planeNormal); + */ - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); + aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); } @@ -53,9 +56,9 @@ void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVecto void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { - btVector3 halfExtents = (aabbMax - aabbMin) * 0.5f; + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); btScalar radius = halfExtents.length(); - btVector3 center = (aabbMax + aabbMin) * 0.5f; + btVector3 center = (aabbMax + aabbMin) * btScalar(0.5); //this is where the triangles are generated, given AABB and plane equation (normal/constant) @@ -85,9 +88,11 @@ void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { + (void)mass; + //moving concave objects not supported - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index 65b30e1ff06..f59cc0c3347 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef STATIC_PLANE_SHAPE_H #define STATIC_PLANE_SHAPE_H -#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "btConcaveShape.h" ///StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index 76b5859be7e..03ca1ae7736 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -23,6 +23,8 @@ btStridingMeshInterface::~btStridingMeshInterface() void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { + (void)aabbMin; + (void)aabbMax; int numtotalphysicsverts = 0; int part,graphicssubparts = getNumSubParts(); const unsigned char * vertexbase; @@ -33,7 +35,7 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde int stride,numverts,numtriangles; int gfxindex; btVector3 triangle[3]; - float* graphicsbase; + btScalar* graphicsbase; btVector3 meshScaling = getScaling(); @@ -50,11 +52,11 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde for (gfxindex=0;gfxindex<numtriangles;gfxindex++) { int* tri_indices= (int*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); + graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); + graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); + graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle,part,gfxindex); } @@ -65,11 +67,11 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde for (gfxindex=0;gfxindex<numtriangles;gfxindex++) { short int* tri_indices= (short int*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); + graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); + graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); + graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle,part,gfxindex); } @@ -83,3 +85,40 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde } } +void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax) +{ + + struct AabbCalculationCallback : public btInternalTriangleIndexCallback + { + btVector3 m_aabbMin; + btVector3 m_aabbMax; + + AabbCalculationCallback() + { + m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + (void)partId; + (void)triangleIndex; + + m_aabbMin.setMin(triangle[0]); + m_aabbMax.setMax(triangle[0]); + m_aabbMin.setMin(triangle[1]); + m_aabbMax.setMax(triangle[1]); + m_aabbMin.setMin(triangle[2]); + m_aabbMax.setMax(triangle[2]); + } + }; + + //first calculate the total aabb for all triangles + AabbCalculationCallback aabbCallback; + aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax); + + aabbMin = aabbCallback.m_aabbMin; + aabbMax = aabbCallback.m_aabbMax; +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index 830cbb28200..d7b354b7855 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef STRIDING_MESHINTERFACE_H #define STRIDING_MESHINTERFACE_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" #include "btTriangleCallback.h" /// PHY_ScalarType enumerates possible scalar types. @@ -38,7 +38,7 @@ class btStridingMeshInterface btVector3 m_scaling; public: - btStridingMeshInterface() :m_scaling(1.f,1.f,1.f) + btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.)) { } @@ -49,6 +49,8 @@ class btStridingMeshInterface void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + ///brute force method to calculate aabb + void calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax); /// get read and write access to a subpart of a triangle mesh /// this subpart has a continuous array of vertices and indices diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 7cb40c4fac1..3aa1eda9964 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -58,6 +58,8 @@ btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const void btBU_Simplex1to4::addVertex(const btPoint3& pt) { m_vertices[m_numVertices++] = pt; + + recalcLocalAabb(); } @@ -176,17 +178,17 @@ int btBU_Simplex1to4::getNumPlanes() const } -void btBU_Simplex1to4::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const +void btBU_Simplex1to4::getPlane(btVector3&, btPoint3& ,int ) const { } -int btBU_Simplex1to4::getIndex(int i) const +int btBU_Simplex1to4::getIndex(int ) const { return 0; } -bool btBU_Simplex1to4::isInside(const btPoint3& pt,btScalar tolerance) const +bool btBU_Simplex1to4::isInside(const btPoint3& ,btScalar ) const { return false; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h index 9e17a248f84..94bc4ec0fa5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -17,8 +17,8 @@ subject to the following restrictions: #define BU_SIMPLEX_1TO4_SHAPE -#include <BulletCollision/CollisionShapes/btPolyhedralConvexShape.h> -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "btPolyhedralConvexShape.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" ///BU_Simplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index 9a623403846..3805c519d22 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -17,8 +17,7 @@ subject to the following restrictions: #define BT_TRIANGLE_BUFFER_H #include "btTriangleCallback.h" -//#include <vector> -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btAlignedObjectArray.h" struct btTriangle { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h index 7b2337498ec..fbb87bc4fd8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef TRIANGLE_CALLBACK_H #define TRIANGLE_CALLBACK_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" class btTriangleCallback diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index 7400d372bee..00847861cf1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -15,15 +15,15 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" -btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride) +btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride) { btIndexedMesh mesh; mesh.m_numTriangles = numTriangles; - mesh.m_triangleIndexBase = triangleIndexBase; + mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase; mesh.m_triangleIndexStride = triangleIndexStride; mesh.m_numVertices = numVertices; - mesh.m_vertexBase = vertexBase; + mesh.m_vertexBase = (const unsigned char *)vertexBase; mesh.m_vertexStride = vertexStride; addIndexedMesh(mesh); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 3ec827c03d9..6ab6a762b39 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -17,40 +17,44 @@ subject to the following restrictions: #define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H #include "btStridingMeshInterface.h" -#include <LinearMath/btAlignedObjectArray.h> +#include "../../LinearMath/btAlignedObjectArray.h" ///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements ///instead of the number of indices, we pass the number of triangles ///todo: explain with pictures -struct btIndexedMesh - { - int m_numTriangles; - int* m_triangleIndexBase; - int m_triangleIndexStride; - int m_numVertices; - float* m_vertexBase; - int m_vertexStride; - }; +ATTRIBUTE_ALIGNED16( struct) btIndexedMesh +{ + int m_numTriangles; + const unsigned char * m_triangleIndexBase; + int m_triangleIndexStride; + int m_numVertices; + const unsigned char * m_vertexBase; + int m_vertexStride; + int pad[2]; +} +; + + +typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray; ///TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. ///Additional meshes can be added using addIndexedMesh ///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. -class btTriangleIndexVertexArray : public btStridingMeshInterface +ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface { - btAlignedObjectArray<btIndexedMesh> m_indexedMeshes; + IndexedMeshArray m_indexedMeshes; + int m_pad[3]; public: - - btTriangleIndexVertexArray() { } //just to be backwards compatible - btTriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride); + btTriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); void addIndexedMesh(const btIndexedMesh& mesh) { @@ -64,19 +68,30 @@ public: /// unLockVertexBase finishes the access to a subpart of the triangle mesh /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished - virtual void unLockVertexBase(int subpart) {} + virtual void unLockVertexBase(int subpart) {(void)subpart;} - virtual void unLockReadOnlyVertexBase(int subpart) const {} + virtual void unLockReadOnlyVertexBase(int subpart) const {(void)subpart;} /// getNumSubParts returns the number of seperate subparts /// each subpart has a continuous array of vertices and indices virtual int getNumSubParts() const { return (int)m_indexedMeshes.size(); } - - virtual void preallocateVertices(int numverts){} - virtual void preallocateIndices(int numindices){} -}; + IndexedMeshArray& getIndexedMeshArray() + { + return m_indexedMeshes; + } + + const IndexedMeshArray& getIndexedMeshArray() const + { + return m_indexedMeshes; + } + + virtual void preallocateVertices(int numverts){(void) numverts;} + virtual void preallocateIndices(int numindices){(void) numindices;} + +} +; #endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 489fe1bbcaa..98c54ef45f8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -16,7 +16,6 @@ subject to the following restrictions: #include "btTriangleMesh.h" #include <assert.h> -static int myindices[3] = {0,1,2}; btTriangleMesh::btTriangleMesh () { @@ -25,31 +24,31 @@ btTriangleMesh::btTriangleMesh () void btTriangleMesh::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) { - numverts = 3; - *vertexbase = (unsigned char*)&m_triangles[subpart]; + (void)subpart; + numverts = m_vertices.size(); + *vertexbase = (unsigned char*)&m_vertices[0]; type = PHY_FLOAT; stride = sizeof(btVector3); - - numfaces = 1; - *indexbase = (unsigned char*) &myindices[0]; + numfaces = m_indices.size()/3; + *indexbase = (unsigned char*) &m_indices[0]; indicestype = PHY_INTEGER; - indexstride = sizeof(int); + indexstride = 3*sizeof(int); } void btTriangleMesh::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const { - numverts = 3; - *vertexbase = (unsigned char*)&m_triangles[subpart]; + (void)subpart; + numverts = m_vertices.size(); + *vertexbase = (unsigned char*)&m_vertices[0]; type = PHY_FLOAT; stride = sizeof(btVector3); - - numfaces = 1; - *indexbase = (unsigned char*) &myindices[0]; + numfaces = m_indices.size()/3; + *indexbase = (unsigned char*) &m_indices[0]; indicestype = PHY_INTEGER; - indexstride = sizeof(int); + indexstride = 3*sizeof(int); } @@ -57,5 +56,5 @@ void btTriangleMesh::getLockedReadOnlyVertexIndexBase(const unsigned char **vert int btTriangleMesh::getNumSubParts() const { - return m_triangles.size(); + return 1; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 1be03d70436..525f5336b48 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -17,42 +17,37 @@ subject to the following restrictions: #ifndef TRIANGLE_MESH_H #define TRIANGLE_MESH_H -#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" -#include <LinearMath/btVector3.h> -#include "LinearMath/btAlignedObjectArray.h" -struct btMyTriangle -{ - btVector3 m_vert0; - btVector3 m_vert1; - btVector3 m_vert2; -}; +#include "btStridingMeshInterface.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btAlignedObjectArray.h" ///TriangleMesh provides storage for a concave triangle mesh. It can be used as data for the btTriangleMeshShape. class btTriangleMesh : public btStridingMeshInterface { - btAlignedObjectArray<btMyTriangle> m_triangles; + btAlignedObjectArray<btVector3> m_vertices; + btAlignedObjectArray<int> m_indices; public: btTriangleMesh (); void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2) { - btMyTriangle tri; - tri.m_vert0 = vertex0; - tri.m_vert1 = vertex1; - tri.m_vert2 = vertex2; - m_triangles.push_back(tri); + int curIndex = m_indices.size(); + m_vertices.push_back(vertex0); + m_vertices.push_back(vertex1); + m_vertices.push_back(vertex2); + + m_indices.push_back(curIndex++); + m_indices.push_back(curIndex++); + m_indices.push_back(curIndex++); } int getNumTriangles() const { - return m_triangles.size(); + return m_indices.size() / 3; } - const btMyTriangle& getTriangle(int index) const - { - return m_triangles[index]; - } + //StridingMeshInterface interface implementation @@ -62,16 +57,16 @@ class btTriangleMesh : public btStridingMeshInterface /// unLockVertexBase finishes the access to a subpart of the triangle mesh /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished - virtual void unLockVertexBase(int subpart) {} + virtual void unLockVertexBase(int subpart) {(void) subpart;} - virtual void unLockReadOnlyVertexBase(int subpart) const {} + virtual void unLockReadOnlyVertexBase(int subpart) const { (void) subpart;} /// getNumSubParts returns the number of seperate subparts /// each subpart has a continuous array of vertices and indices virtual int getNumSubParts() const; - virtual void preallocateVertices(int numverts){} - virtual void preallocateIndices(int numindices){} + virtual void preallocateVertices(int numverts){(void) numverts;} + virtual void preallocateIndices(int numindices){(void) numindices;} }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index cd2bf7261d1..ed81897b515 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -40,8 +40,8 @@ btTriangleMeshShape::~btTriangleMeshShape() void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { - btVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); btMatrix3x3 abs_b = trans.getBasis().absolute(); @@ -62,11 +62,11 @@ void btTriangleMeshShape::recalcLocalAabb() { for (int i=0;i<3;i++) { - btVector3 vec(0.f,0.f,0.f); - vec[i] = 1.f; + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); m_localAabbMax[i] = tmp[i]+m_collisionMargin; - vec[i] = -1.f; + vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); m_localAabbMin[i] = tmp[i]-m_collisionMargin; } @@ -85,7 +85,7 @@ public: btVector3 m_supportVecLocal; SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) - : m_supportVertexLocal(0.f,0.f,0.f), m_worldTrans(trans) ,m_maxDot(-1e30f) + : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30)) { m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); @@ -93,6 +93,8 @@ public: virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex) { + (void)partId; + (void)triangleIndex; for (int i=0;i<3;i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); @@ -138,8 +140,7 @@ const btVector3& btTriangleMeshShape::getLocalScaling() const void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { - - struct FilteredCallback : public btInternalTriangleIndexCallback + struct FilteredCallback : public btInternalTriangleIndexCallback { btTriangleCallback* m_callback; btVector3 m_aabbMin; @@ -167,18 +168,19 @@ void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const FilteredCallback filterCallback(callback,aabbMin,aabbMax); m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax); - } + void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { + (void)mass; //moving concave objects not supported - assert(0); - inertia.setValue(0.f,0.f,0.f); + btAssert(0); + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } @@ -191,7 +193,7 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co SupportVertexCallback supportCallback(vec,ident); - btVector3 aabbMax(1e30f,1e30f,1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); processAllTriangles(&supportCallback,-aabbMax,aabbMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 4bf000f0c8d..e6173e47640 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -16,17 +16,17 @@ subject to the following restrictions: #ifndef TRIANGLE_MESH_SHAPE_H #define TRIANGLE_MESH_SHAPE_H -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" +#include "btConcaveShape.h" +#include "btStridingMeshInterface.h" ///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. class btTriangleMeshShape : public btConcaveShape { protected: - btStridingMeshInterface* m_meshInterface; btVector3 m_localAabbMin; btVector3 m_localAabbMax; + btStridingMeshInterface* m_meshInterface; public: @@ -58,6 +58,16 @@ public: virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + btStridingMeshInterface* getMeshInterface() + { + return m_meshInterface; + } + + const btStridingMeshInterface* getMeshInterface() const + { + return m_meshInterface; + } + //debugging virtual char* getName()const {return "TRIANGLEMESH";} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index 3284a599ea7..c2e240c051c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define OBB_TRIANGLE_MINKOWSKI_H #include "btConvexShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "btBoxShape.h" class btTriangleShape : public btPolyhedralConvexShape { @@ -57,6 +57,7 @@ public: getVertex((i+1)%3,pb); } + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const { // btAssert(0); @@ -110,14 +111,16 @@ public: virtual void getPlaneEquation(int i, btVector3& planeNormal,btPoint3& planeSupport) const { + (void)i; calcNormal(planeNormal); planeSupport = m_vertices1[0]; } virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) { + (void)mass; btAssert(0); - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } virtual bool isInside(const btPoint3& pt,btScalar tolerance) const @@ -166,7 +169,7 @@ public: { calcNormal(penetrationVector); if (index) - penetrationVector *= -1.f; + penetrationVector *= btScalar(-1.); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index ae3ce42e77f..2c565734e97 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -49,28 +49,28 @@ bool btContinuousConvexCollision::calcTimeOfImpact( /// compute linear and angular velocity for this interval, to interpolate btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocity(fromA,toA,1.f,linVelA,angVelA); - btTransformUtil::calculateVelocity(fromB,toB,1.f,linVelB,angVelB); + btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA); + btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB); btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); btScalar boundingRadiusB = m_convexB->getAngularMotionDisc(); btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; - float radius = 0.001f; + btScalar radius = btScalar(0.001); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); btVector3 v(1,0,0); int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(0.f,0.f,0.f); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; - float lastLambda = lambda; - //float epsilon = 0.001f; + btScalar lastLambda = lambda; + //btScalar epsilon = btScalar(0.001); int numIter = 0; //first solution, using GJK @@ -79,8 +79,8 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(0.0f); - raySphere.setMargin(0.f); + btSphereShape raySphere(btScalar(0.0)); + raySphere.setMargin(btScalar(0.)); // result.drawCoordSystem(sphereTr); @@ -116,23 +116,23 @@ bool btContinuousConvexCollision::calcTimeOfImpact( if (numIter > maxIter) return false; //todo: report a failure - float dLambda = 0.f; + btScalar dLambda = btScalar(0.); //calculate safe moving fraction from distance / (linear+rotational velocity) - //float clippedDist = GEN_min(angularConservativeRadius,dist); - //float clippedDist = dist; + //btScalar clippedDist = GEN_min(angularConservativeRadius,dist); + //btScalar clippedDist = dist; - float projectedLinearVelocity = (linVelB-linVelA).dot(n); + btScalar projectedLinearVelocity = (linVelB-linVelA).dot(n); dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); lambda = lambda + dLambda; - if (lambda > 1.f) + if (lambda > btScalar(1.)) return false; - if (lambda < 0.f) + if (lambda < btScalar(0.)) return false; //todo: next check with relative epsilon @@ -159,7 +159,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { - if (pointCollector.m_distance < 0.f) + if (pointCollector.m_distance < btScalar(0.)) { //degenerate ?! result.m_fraction = lastLambda; @@ -188,9 +188,9 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //todo: //if movement away from normal, discard result btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin(); - if (result.m_fraction < 1.f) + if (result.m_fraction < btScalar(1.)) { - if (move.dot(result.m_normal) <= 0.f) + if (move.dot(result.m_normal) <= btScalar(0.)) { } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index 4258d829cca..3101b59993d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -17,11 +17,11 @@ subject to the following restrictions: #ifndef CONVEX_CAST_H #define CONVEX_CAST_H -#include <LinearMath/btTransform.h> -#include <LinearMath/btVector3.h> -#include <LinearMath/btScalar.h> +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btScalar.h" class btMinkowskiSumShape; -#include "LinearMath/btIDebugDraw.h" +#include "../../LinearMath/btIDebugDraw.h" /// btConvexCast is an interface for Casting class btConvexCast @@ -37,11 +37,11 @@ public: { //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0; - virtual void DebugDraw(btScalar fraction) {} - virtual void drawCoordSystem(const btTransform& trans) {} + virtual void DebugDraw(btScalar fraction) {(void)fraction;} + virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} CastResult() - :m_fraction(1e30f), + :m_fraction(btScalar(1e30)), m_debugDrawer(0) { } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 188c8258c8e..7caeba4be45 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -21,7 +21,7 @@ class btStackAlloc; class btVector3; #include "btSimplexSolverInterface.h" class btConvexShape; -#include "LinearMath/btPoint3.h" +#include "../../LinearMath/btPoint3.h" class btTransform; ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index e9db97d1b13..15000c1ab61 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H #define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" class btStackAlloc; /// This interface is made to be used by an iterative approach to do TimeOfImpact calculations @@ -30,19 +30,18 @@ struct btDiscreteCollisionDetectorInterface struct Result { - void operator delete(void* ptr) {}; virtual ~Result(){} ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)=0; + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(1e30f), + :m_maximumDistanceSquared(btScalar(1e30)), m_stackAlloc(0) { } @@ -69,13 +68,13 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result btVector3 m_closestPointInB; btScalar m_distance; //negative means penetration ! - btStorageResult() : m_distance(1e30f) + btStorageResult() : m_distance(btScalar(1e30)) { } virtual ~btStorageResult() {}; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { if (depth < m_distance) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index bf465b61857..93edffeafd6 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -60,9 +60,9 @@ bool btGjkConvexCast::calcTimeOfImpact( - float radius = 0.01f; + btScalar radius = btScalar(0.01); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); btVector3 s = rayFromLocalA.getOrigin(); btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin(); btVector3 x = s; @@ -71,7 +71,7 @@ bool btGjkConvexCast::calcTimeOfImpact( bool hasResult = false; btVector3 c; - float lastLambda = lambda; + btScalar lastLambda = lambda; //first solution, using GJK @@ -81,8 +81,8 @@ bool btGjkConvexCast::calcTimeOfImpact( btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(0.0f); - raySphere.setMargin(0.f); + btSphereShape raySphere(btScalar(0.0)); + raySphere.setMargin(btScalar(0.)); btTransform sphereTr; sphereTr.setIdentity(); @@ -112,7 +112,7 @@ bool btGjkConvexCast::calcTimeOfImpact( if (dist < radius) { //penetration - lastLambda = 1.f; + lastLambda = btScalar(1.); } //not close enough @@ -143,7 +143,7 @@ bool btGjkConvexCast::calcTimeOfImpact( gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { - if (pointCollector.m_distance < 0.f) + if (pointCollector.m_distance < btScalar(0.)) { //degeneracy, report a hit result.m_fraction = lastLambda; @@ -160,7 +160,7 @@ bool btGjkConvexCast::calcTimeOfImpact( } - if (lastLambda < 1.f) + if (lastLambda < btScalar(1.)) { result.m_fraction = lastLambda; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index 66b34b88363..3905c45e6d6 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -18,9 +18,9 @@ subject to the following restrictions: #ifndef GJK_CONVEX_CAST_H #define GJK_CONVEX_CAST_H -#include <BulletCollision/CollisionShapes/btCollisionMargin.h> +#include "../CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" #include "btConvexCast.h" class btConvexShape; class btMinkowskiSumShape; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h index 359829f8f31..759b30bb17f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h @@ -21,7 +21,7 @@ Nov.2006 #ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ #define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ -#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "../CollisionShapes/btConvexShape.h" class btStackAlloc; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 3d252751c9a..87330493b60 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -26,8 +26,11 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) { + (void)debugDraw; + (void)v; + (void)simplexSolver; - const btScalar radialmargin(0.f); + const btScalar radialmargin(btScalar(0.)); btGjkEpaSolver::sResults results; if(btGjkEpaSolver::Collide( pConvexA,transformA, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 0ce04dcfad8..f1f3f7f7f6c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #endif //must be above the machine epsilon -#define REL_ERROR2 1.0e-6f +#define REL_ERROR2 btScalar(1.0e-6) //temp globals, to improve GJK/EPA/penetration calculations int gNumDeepPenetrationChecks = 0; @@ -36,7 +36,7 @@ int gNumGjkChecks = 0; btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(0.f,0.f,1.f), +:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), @@ -49,25 +49,25 @@ m_catchDegeneracies(1) void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) { - btScalar distance=0.f; - btVector3 normalInB(0.f,0.f,0.f); + btScalar distance=btScalar(0.); + btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 pointOnA,pointOnB; btTransform localTransA = input.m_transformA; btTransform localTransB = input.m_transformB; - btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * 0.5f; + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; - float marginA = m_minkowskiA->getMargin(); - float marginB = m_minkowskiB->getMargin(); + btScalar marginA = m_minkowskiA->getMargin(); + btScalar marginB = m_minkowskiB->getMargin(); gNumGjkChecks++; //for CCD we don't use margins if (m_ignoreMargin) { - marginA = 0.f; - marginB = 0.f; + marginA = btScalar(0.); + marginB = btScalar(0.); } m_curIter = 0; @@ -83,7 +83,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { btScalar squaredDistance = SIMD_INFINITY; - btScalar delta = 0.f; + btScalar delta = btScalar(0.); btScalar margin = marginA + marginB; @@ -91,7 +91,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& m_simplexSolver->reset(); - while (true) + for ( ; ; ) + //while (true) { btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); @@ -120,12 +121,12 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& break; } // are we getting any closer ? - float f0 = squaredDistance - delta; - float f1 = squaredDistance * REL_ERROR2; + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * REL_ERROR2; if (f0 <= f1) { - if (f0 <= 0.f) + if (f0 <= btScalar(0.)) { m_degenerateSimplex = 2; } @@ -191,7 +192,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = pointOnA-pointOnB; - float lenSqr = m_cachedSeparatingAxis.length2(); + btScalar lenSqr = m_cachedSeparatingAxis.length2(); //valid normal if (lenSqr < 0.0001) { @@ -199,14 +200,14 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); normalInB *= rlen; //normalize btScalar s = btSqrt(squaredDistance); btAssert(s > btScalar(0.0)); pointOnA -= m_cachedSeparatingAxis * (marginA / s); pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((1.f/rlen) - margin); + distance = ((btScalar(1.)/rlen) - margin); isValid = true; m_lastUsedMethod = 1; @@ -243,11 +244,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (isValid2) { btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - float lenSqr = tmpNormalInB.length2(); + btScalar lenSqr = tmpNormalInB.length2(); if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); - float distance2 = -(tmpPointOnA-tmpPointOnB).length(); + btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); //only replace valid penetrations when the result is deeper (check) if (!isValid || (distance2 < distance)) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 09c1669bd78..af0fe32f6c7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -20,9 +20,8 @@ subject to the following restrictions: #define GJK_PAIR_DETECTOR_H #include "btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" - -#include <BulletCollision/CollisionShapes/btCollisionMargin.h> +#include "../../LinearMath/btPoint3.h" +#include "../CollisionShapes/btCollisionMargin.h" class btConvexShape; #include "btSimplexSolverInterface.h" diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 00a9206fef0..f6a893151da 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef MANIFOLD_CONTACT_POINT_H #define MANIFOLD_CONTACT_POINT_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransformUtil.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransformUtil.h" @@ -29,7 +29,8 @@ class btManifoldPoint { public: btManifoldPoint() - :m_userPersistentData(0) + :m_userPersistentData(0), + m_lifeTime(0) { } @@ -40,8 +41,8 @@ class btManifoldPoint m_localPointB( pointB ), m_normalWorldOnB( normal ), m_distance1( distance ), - m_combinedFriction(0.f), - m_combinedRestitution(0.f), + m_combinedFriction(btScalar(0.)), + m_combinedRestitution(btScalar(0.)), m_userPersistentData(0), m_lifeTime(0) { @@ -58,16 +59,16 @@ class btManifoldPoint btVector3 m_positionWorldOnA; btVector3 m_normalWorldOnB; - float m_distance1; - float m_combinedFriction; - float m_combinedRestitution; + btScalar m_distance1; + btScalar m_combinedFriction; + btScalar m_combinedRestitution; - void* m_userPersistentData; + mutable void* m_userPersistentData; int m_lifeTime;//lifetime of the contactpoint in frames - float getDistance() const + btScalar getDistance() const { return m_distance1; } @@ -76,17 +77,17 @@ class btManifoldPoint return m_lifeTime; } - btVector3 getPositionWorldOnA() { + const btVector3& getPositionWorldOnA() const { return m_positionWorldOnA; // return m_positionWorldOnB + m_normalWorldOnB * m_distance1; } - const btVector3& getPositionWorldOnB() + const btVector3& getPositionWorldOnB() const { return m_positionWorldOnB; } - void setDistance(float dist) + void setDistance(btScalar dist) { m_distance1 = dist; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index de6749a9b72..c4bab3a134a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -25,48 +25,48 @@ subject to the following restrictions: #define NUM_UNITSPHERE_POINTS 42 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = { -btVector3(0.000000f , -0.000000f,-1.000000f), -btVector3(0.723608f , -0.525725f,-0.447219f), -btVector3(-0.276388f , -0.850649f,-0.447219f), -btVector3(-0.894426f , -0.000000f,-0.447216f), -btVector3(-0.276388f , 0.850649f,-0.447220f), -btVector3(0.723608f , 0.525725f,-0.447219f), -btVector3(0.276388f , -0.850649f,0.447220f), -btVector3(-0.723608f , -0.525725f,0.447219f), -btVector3(-0.723608f , 0.525725f,0.447219f), -btVector3(0.276388f , 0.850649f,0.447219f), -btVector3(0.894426f , 0.000000f,0.447216f), -btVector3(-0.000000f , 0.000000f,1.000000f), -btVector3(0.425323f , -0.309011f,-0.850654f), -btVector3(-0.162456f , -0.499995f,-0.850654f), -btVector3(0.262869f , -0.809012f,-0.525738f), -btVector3(0.425323f , 0.309011f,-0.850654f), -btVector3(0.850648f , -0.000000f,-0.525736f), -btVector3(-0.525730f , -0.000000f,-0.850652f), -btVector3(-0.688190f , -0.499997f,-0.525736f), -btVector3(-0.162456f , 0.499995f,-0.850654f), -btVector3(-0.688190f , 0.499997f,-0.525736f), -btVector3(0.262869f , 0.809012f,-0.525738f), -btVector3(0.951058f , 0.309013f,0.000000f), -btVector3(0.951058f , -0.309013f,0.000000f), -btVector3(0.587786f , -0.809017f,0.000000f), -btVector3(0.000000f , -1.000000f,0.000000f), -btVector3(-0.587786f , -0.809017f,0.000000f), -btVector3(-0.951058f , -0.309013f,-0.000000f), -btVector3(-0.951058f , 0.309013f,-0.000000f), -btVector3(-0.587786f , 0.809017f,-0.000000f), -btVector3(-0.000000f , 1.000000f,-0.000000f), -btVector3(0.587786f , 0.809017f,-0.000000f), -btVector3(0.688190f , -0.499997f,0.525736f), -btVector3(-0.262869f , -0.809012f,0.525738f), -btVector3(-0.850648f , 0.000000f,0.525736f), -btVector3(-0.262869f , 0.809012f,0.525738f), -btVector3(0.688190f , 0.499997f,0.525736f), -btVector3(0.525730f , 0.000000f,0.850652f), -btVector3(0.162456f , -0.499995f,0.850654f), -btVector3(-0.425323f , -0.309011f,0.850654f), -btVector3(-0.425323f , 0.309011f,0.850654f), -btVector3(0.162456f , 0.499995f,0.850654f) +btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), +btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), +btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), +btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), +btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), +btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), +btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), +btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), +btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), +btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), +btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), +btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), +btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), +btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), +btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), +btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), +btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), +btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), +btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), +btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), +btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), +btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), +btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), +btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), +btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), +btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), +btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), +btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), +btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), +btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) }; @@ -78,6 +78,9 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s ) { + (void)stackAlloc; + (void)v; + struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { @@ -88,13 +91,17 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; - float m_depth; + btScalar m_depth; bool m_hasResult; virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) { + (void)partId0; + (void)index0; + (void)partId1; + (void)index1; } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { m_normalOnBInWorld = normalOnBInWorld; m_pointInWorld = pointInWorld; @@ -104,7 +111,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s }; //just take fixed number of orientation, and sample the penetration depth in that direction - float minProj = 1e30f; + btScalar minProj = btScalar(1e30); btVector3 minNorm; btVector3 minVertex; btVector3 minA,minB; @@ -180,7 +187,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; - float delta = norm.dot(w); + btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) { @@ -234,7 +241,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; - float delta = norm.dot(w); + btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) { @@ -251,7 +258,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minA += minNorm*convexA->getMargin(); minB -= minNorm*convexB->getMargin(); //no penetration - if (minProj < 0.f) + if (minProj < btScalar(0.)) return false; minProj += (convexA->getMargin() + convexB->getMargin()); @@ -268,7 +275,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s debugDraw->drawLine(minA,minB,color); color = btVector3 (1,1,1); btVector3 vec = minB-minA; - float prj2 = minNorm.dot(vec); + btScalar prj2 = minNorm.dot(vec); debugDraw->drawLine(minA,minA+(minNorm*minProj),color); } @@ -292,16 +299,16 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = 1e30f;//minProj; + input.m_maximumDistanceSquared = btScalar(1e30);//minProj; btIntermediateResult res; gjkdet.getClosestPoints(input,res,debugDraw); - float correctedMinNorm = minProj - res.m_depth; + btScalar correctedMinNorm = minProj - res.m_depth; //the penetration depth is over-estimated, relax it - float penetration_relaxation= 1.f; + btScalar penetration_relaxation= btScalar(1.); minNorm*=penetration_relaxation; if (res.m_hasResult) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index ee2be163063..08cb3ed334d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include <assert.h> -float gContactBreakingThreshold = 0.02f; +btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; @@ -100,7 +100,7 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) int maxPenetrationIndex = -1; #define KEEP_DEEPEST_POINT 1 #ifdef KEEP_DEEPEST_POINT - float maxPenetration = pt.getDistance(); + btScalar maxPenetration = pt.getDistance(); for (int i=0;i<4;i++) { if (m_pointCache[i].getDistance() < maxPenetration) @@ -111,7 +111,7 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) } #endif //KEEP_DEEPEST_POINT - btScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f); + btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); if (maxPenetrationIndex != 0) { btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; @@ -193,7 +193,7 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) replaceContactPoint(newPoint,insertIndex); } -float btPersistentManifold::getContactBreakingThreshold() const +btScalar btPersistentManifold::getContactBreakingThreshold() const { return gContactBreakingThreshold; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index c2372b648f6..a5918b84db3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -17,14 +17,14 @@ subject to the following restrictions: #define PERSISTENT_MANIFOLD_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" #include "btManifoldPoint.h" struct btCollisionResult; ///contact breaking and merging threshold -extern float gContactBreakingThreshold; +extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); extern ContactDestroyedCallback gContactDestroyedCallback; @@ -36,7 +36,7 @@ extern ContactDestroyedCallback gContactDestroyedCallback; ///btPersistentManifold maintains contact points, and reduces them to 4. ///It does contact filtering/contact reduction. -class btPersistentManifold +ATTRIBUTE_ALIGNED16( class) btPersistentManifold { btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; @@ -97,7 +97,7 @@ public: } /// todo: get this margin from the current physics / collision environment - float getContactBreakingThreshold() const; + btScalar getContactBreakingThreshold() const; int getCacheEntry(const btManifoldPoint& newPoint) const; @@ -108,18 +108,36 @@ public: clearUserCache(m_pointCache[index]); int lastUsedIndex = getNumContacts() - 1; - m_pointCache[index] = m_pointCache[lastUsedIndex]; - //get rid of duplicated userPersistentData pointer - m_pointCache[lastUsedIndex].m_userPersistentData = 0; +// m_pointCache[index] = m_pointCache[lastUsedIndex]; + if(index != lastUsedIndex) + { + m_pointCache[index] = m_pointCache[lastUsedIndex]; + //get rid of duplicated userPersistentData pointer + m_pointCache[lastUsedIndex].m_userPersistentData = 0; + } + + btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); m_cachedPoints--; } void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) { - assert(validContactDistance(newPoint)); + btAssert(validContactDistance(newPoint)); - clearUserCache(m_pointCache[insertIndex]); +#define MAINTAIN_PERSISTENCY 1 +#ifdef MAINTAIN_PERSISTENCY + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btAssert(lifeTime>=0); + void* cache = m_pointCache[insertIndex].m_userPersistentData; m_pointCache[insertIndex] = newPoint; + + m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_lifeTime = lifeTime; +#else + clearUserCache(m_pointCache[insertIndex]); + m_pointCache[insertIndex] = newPoint; + +#endif } bool validContactDistance(const btManifoldPoint& pt) const @@ -133,7 +151,10 @@ public: -}; +} +; + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index a51e2d5e13b..6262f44b9f1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -31,16 +31,20 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result bool m_hasResult; btPointCollector () - : m_distance(1e30f),m_hasResult(false) + : m_distance(btScalar(1e30)),m_hasResult(false) { } virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) { + (void)partId0; + (void)index0; + (void)partId1; + (void)index1; //?? } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { if (depth< m_distance) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 88ee005792c..31b91467777 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -20,7 +20,7 @@ btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const : m_from(from), m_to(to), - m_hitFraction(1.f) + m_hitFraction(btScalar(1.)) { } @@ -40,19 +40,19 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); - const float dist = vert0.dot(triangleNormal); - float dist_a = triangleNormal.dot(m_from) ; + const btScalar dist = vert0.dot(triangleNormal); + btScalar dist_a = triangleNormal.dot(m_from) ; dist_a-= dist; - float dist_b = triangleNormal.dot(m_to); + btScalar dist_b = triangleNormal.dot(m_to); dist_b -= dist; - if ( dist_a * dist_b >= 0.0f) + if ( dist_a * dist_b >= btScalar(0.0) ) { return ; // same sign } - const float proj_length=dist_a-dist_b; - const float distance = (dist_a)/(proj_length); + const btScalar proj_length=dist_a-dist_b; + const btScalar distance = (dist_a)/(proj_length); // Now we have the intersection point on the plane, we'll see if it's inside the triangle // Add an epsilon as a tolerance for the raycast, // in case the ray hits exacly on the edge of the triangle. @@ -62,27 +62,27 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, { - float edge_tolerance =triangleNormal.length2(); - edge_tolerance *= -0.0001f; + btScalar edge_tolerance =triangleNormal.length2(); + edge_tolerance *= btScalar(-0.0001); btVector3 point; point.setInterpolate3( m_from, m_to, distance); { btVector3 v0p; v0p = vert0 - point; btVector3 v1p; v1p = vert1 - point; btVector3 cp0; cp0 = v0p.cross( v1p ); - if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) { btVector3 v2p; v2p = vert2 - point; btVector3 cp1; cp1 = v1p.cross( v2p); - if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) { btVector3 cp2; cp2 = v2p.cross(v0p); - if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) { if ( dist_a > 0 ) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index fbb51d30522..a0bbc9f8fe9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef RAYCAST_TRI_CALLBACK_H #define RAYCAST_TRI_CALLBACK_H -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "../CollisionShapes/btTriangleCallback.h" struct btBroadphaseProxy; @@ -28,13 +28,13 @@ public: btVector3 m_from; btVector3 m_to; - float m_hitFraction; + btScalar m_hitFraction; btTriangleRaycastCallback(const btVector3& from,const btVector3& to); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0; + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index cf65f46505b..58393b2eab9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -18,8 +18,8 @@ subject to the following restrictions: #ifndef SIMPLEX_SOLVER_INTERFACE_H #define SIMPLEX_SOLVER_INTERFACE_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btPoint3.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btPoint3.h" #define NO_VIRTUAL_INTERFACE 1 #ifdef NO_VIRTUAL_INTERFACE diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index dc995ca1f72..1ab336b4d54 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -28,8 +28,11 @@ m_convexA(convexA),m_convexB(convexB) ///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. ///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_ITERATIONS 64 +#else #define MAX_ITERATIONS 32 - +#endif bool btSubsimplexConvexCast::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, @@ -52,9 +55,9 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( convex->setTransformB(btTransform(rayFromLocalA.getBasis())); - //float radius = 0.01f; + //btScalar radius = btScalar(0.01); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); //todo: need to verify this: //because of minkowski difference, we need the inverse direction @@ -69,27 +72,30 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(0.f,0.f,0.f); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; - float lastLambda = lambda; - + btScalar lastLambda = lambda; - float dist2 = v.length2(); - float epsilon = 0.0001f; + btScalar dist2 = v.length2(); +#ifdef BT_USE_DOUBLE_PRECISION + btScalar epsilon = btScalar(0.0001); +#else + btScalar epsilon = btScalar(0.0001); +#endif //BT_USE_DOUBLE_PRECISION btVector3 w,p; - float VdotR; + btScalar VdotR; while ( (dist2 > epsilon) && maxIter--) { p = convex->localGetSupportingVertex( v); w = x - p; - float VdotW = v.dot(w); + btScalar VdotW = v.dot(w); - if ( VdotW > 0.f) + if ( VdotW > btScalar(0.)) { VdotR = v.dot(r); @@ -117,7 +123,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( //printf("numverts = %i\n",m_simplexSolver->numVertices()); } else { - dist2 = 0.f; + dist2 = btScalar(0.); } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 23d66a3bbc8..105b7eccefa 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -70,7 +70,7 @@ void btVoronoiSimplexSolver::reset() m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(1e30f,1e30f,1e30f); + m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_cachedBC.reset(); } @@ -109,7 +109,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() m_cachedP2 = m_simplexPointsQ[0]; m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(1.f,0.f,0.f,0.f); + m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); m_cachedValidClosest = m_cachedBC.isValid(); break; }; @@ -120,13 +120,13 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() const btVector3& to = m_simplexVectorW[1]; btVector3 nearest; - btVector3 p (0.f,0.f,0.f); + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 diff = p - from; btVector3 v = to - from; - float t = v.dot(diff); + btScalar t = v.dot(diff); if (t > 0) { - float dotVV = v.dot(v); + btScalar dotVV = v.dot(v); if (t < dotVV) { t /= dotVV; diff -= t*v; @@ -156,38 +156,36 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() m_cachedValidClosest = m_cachedBC.isValid(); break; } - case 3: - { - //closest point origin from triangle - btVector3 p (0.f,0.f,0.f); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; + case 3: + { + //closest point origin from triangle + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - closestPtPointTriangle(p,a,b,c,m_cachedBC); - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + closestPtPointTriangle(p,a,b,c,m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; - m_cachedV = m_cachedP1-m_cachedP2; + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; - reduceVertices (m_cachedBC.m_usedVertices); - m_cachedValidClosest = m_cachedBC.isValid(); + m_cachedV = m_cachedP1-m_cachedP2; - break; + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; } case 4: { - btVector3 p (0.f,0.f,0.f); + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); const btVector3& a = m_simplexVectorW[0]; const btVector3& b = m_simplexVectorW[1]; @@ -222,7 +220,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero - m_cachedV.setValue(0.f,0.f,0.f); + m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } break; } @@ -256,7 +254,7 @@ bool btVoronoiSimplexSolver::closest(btVector3& v) btScalar btVoronoiSimplexSolver::maxVertex() { int i, numverts = numVertices(); - btScalar maxV = 0.f; + btScalar maxV = btScalar(0.); for (i=0;i<numverts;i++) { btScalar curLen2 = m_simplexVectorW[i].length2(); @@ -288,7 +286,7 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) { bool found = false; int i, numverts = numVertices(); - //btScalar maxV = 0.f; + //btScalar maxV = btScalar(0.); //w is in the current (reduced) simplex for (i=0;i<numverts;i++) @@ -335,9 +333,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP btVector3 ab = b - a; btVector3 ac = c - a; btVector3 ap = p - a; - float d1 = ab.dot(ap); - float d2 = ac.dot(ap); - if (d1 <= 0.0f && d2 <= 0.0f) + btScalar d1 = ab.dot(ap); + btScalar d2 = ac.dot(ap); + if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) { result.m_closestPointOnSimplex = a; result.m_usedVertices.usedVertexA = true; @@ -347,9 +345,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP // Check if P in vertex region outside B btVector3 bp = p - b; - float d3 = ab.dot(bp); - float d4 = ac.dot(bp); - if (d3 >= 0.0f && d4 <= d3) + btScalar d3 = ab.dot(bp); + btScalar d4 = ac.dot(bp); + if (d3 >= btScalar(0.0) && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.usedVertexB = true; @@ -358,9 +356,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP return true; // b; // barycentric coordinates (0,1,0) } // Check if P in edge region of AB, if so return projection of P onto AB - float vc = d1*d4 - d3*d2; - if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { - float v = d1 / (d1 - d3); + btScalar vc = d1*d4 - d3*d2; + if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { + btScalar v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; @@ -371,9 +369,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP // Check if P in vertex region outside C btVector3 cp = p - c; - float d5 = ab.dot(cp); - float d6 = ac.dot(cp); - if (d6 >= 0.0f && d5 <= d6) + btScalar d5 = ab.dot(cp); + btScalar d6 = ac.dot(cp); + if (d6 >= btScalar(0.0) && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.usedVertexC = true; @@ -382,9 +380,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // Check if P in edge region of AC, if so return projection of P onto AC - float vb = d5*d2 - d1*d6; - if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { - float w = d2 / (d2 - d6); + btScalar vb = d5*d2 - d1*d6; + if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { + btScalar w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexC = true; @@ -394,9 +392,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // Check if P in edge region of BC, if so return projection of P onto BC - float va = d3*d6 - d5*d4; - if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { - float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + btScalar va = d3*d6 - d5*d4; + if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { + btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.usedVertexB = true; @@ -407,9 +405,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - float denom = 1.0f / (va + vb + vc); - float v = vb * denom; - float w = vc * denom; + btScalar denom = btScalar(1.0) / (va + vb + vc); + btScalar v = vb * denom; + btScalar w = vc * denom; result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices.usedVertexA = true; @@ -418,7 +416,7 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP result.setBarycentricCoordinates(1-v-w,v,w); return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w +// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w } @@ -431,18 +429,26 @@ int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint { btVector3 normal = (b-a).cross(c-a); - float signp = (p - a).dot(normal); // [AP AB AC] - float signd = (d - a).dot( normal); // [AD AB AC] + btScalar signp = (p - a).dot(normal); // [AP AB AC] + btScalar signd = (d - a).dot( normal); // [AD AB AC] #ifdef CATCH_DEGENERATE_TETRAHEDRON - if (signd * signd < (1e-4f * 1e-4f)) +#ifdef BT_USE_DOUBLE_PRECISION +if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + { + return -1; + } +#else + if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) { // printf("affine dependent/degenerate\n");// return -1; } #endif + +#endif // Points on opposite sides if expression signs are opposite - return signp * signd < 0.f; + return signp * signd < btScalar(0.); } @@ -475,14 +481,14 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const } - float bestSqDist = FLT_MAX; + btScalar bestSqDist = FLT_MAX; // If point outside face abc then compute closest point on abc if (pointOutsideABC) { closestPtPointTriangle(p, a, b, c,tempResult); btPoint3 q = tempResult.m_closestPointOnSimplex; - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; @@ -510,13 +516,14 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; finalResult.setBarycentricCoordinates( @@ -537,15 +544,16 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; + + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTC], @@ -563,15 +571,16 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const closestPtPointTriangle(p, b, d, c,tempResult); btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); + // finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( 0, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 8b743996915..356d335bc93 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -55,7 +55,7 @@ struct btSubSimplexClosestResult //stores the simplex vertex-usage, using the MASK, // if m_usedVertices & MASK then the related vertex is used btUsageBitfield m_usedVertices; - float m_barycentricCoords[4]; + btScalar m_barycentricCoords[4]; bool m_degenerate; void reset() @@ -66,15 +66,15 @@ struct btSubSimplexClosestResult } bool isValid() { - bool valid = (m_barycentricCoords[0] >= 0.f) && - (m_barycentricCoords[1] >= 0.f) && - (m_barycentricCoords[2] >= 0.f) && - (m_barycentricCoords[3] >= 0.f); + bool valid = (m_barycentricCoords[0] >= btScalar(0.)) && + (m_barycentricCoords[1] >= btScalar(0.)) && + (m_barycentricCoords[2] >= btScalar(0.)) && + (m_barycentricCoords[3] >= btScalar(0.)); return valid; } - void setBarycentricCoordinates(float a=0.f,float b=0.f,float c=0.f,float d=0.f) + void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.)) { m_barycentricCoords[0] = a; m_barycentricCoords[1] = b; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index ce90f798c04..7e8458c2c7b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -16,12 +16,16 @@ subject to the following restrictions: #ifndef CONSTRAINT_SOLVER_H #define CONSTRAINT_SOLVER_H +#include "../../LinearMath/btScalar.h" + class btPersistentManifold; class btRigidBody; +class btCollisionObject; class btTypedConstraint; struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; +class btStackAlloc; /// btConstraintSolver provides solver interface class btConstraintSolver @@ -31,7 +35,7 @@ public: virtual ~btConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 1b85a0eea42..bb3fe832592 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -30,13 +30,17 @@ subject to the following restrictions: //bilateral constraint between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep) + btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) { - float normalLenSqr = normal.length2(); - ASSERT2(fabs(normalLenSqr) < 1.1f); - if (normalLenSqr > 1.1f) + (void)timeStep; + (void)distance; + + + btScalar normalLenSqr = normal.length2(); + ASSERT2(btFabs(normalLenSqr) < btScalar(1.1)); + if (normalLenSqr > btScalar(1.1)) { - impulse = 0.f; + impulse = btScalar(0.); return; } btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); @@ -54,24 +58,24 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, body2.getInvInertiaDiagLocal(),body2.getInvMass()); btScalar jacDiagAB = jac.getDiagonal(); - btScalar jacDiagABInv = 1.f / jacDiagAB; + btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; btScalar rel_vel = jac.getRelativeVelocity( body1.getLinearVelocity(), body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - float a; + btScalar a; a=jacDiagABInv; rel_vel = normal.dot(vel); //todo: move this into proper structure - btScalar contactDamping = 0.2f; + btScalar contactDamping = btScalar(0.2); #ifdef ONLY_USE_LINEAR_MASS - btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); impulse = - contactDamping * rel_vel * massTerm; #else btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; @@ -82,19 +86,20 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, //response between two dynamic objects with friction -float resolveSingleCollision( +btScalar resolveSingleCollision( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { - const btVector3& pos1 = contactPoint.getPositionWorldOnA(); - const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + const btVector3& pos1_ = contactPoint.getPositionWorldOnA(); + const btVector3& pos2_ = contactPoint.getPositionWorldOnB(); const btVector3& normal = contactPoint.m_normalWorldOnB; - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + //constant over all iterations + btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition(); btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); @@ -102,11 +107,11 @@ float resolveSingleCollision( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar Kfps = 1.f / solverInfo.m_timeStep ; + btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - // float damping = solverInfo.m_damping ; - float Kerp = solverInfo.m_erp; - float Kcor = Kerp *Kfps; + // btScalar damping = solverInfo.m_damping ; + btScalar Kerp = solverInfo.m_erp; + btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); @@ -121,9 +126,9 @@ float resolveSingleCollision( btScalar normalImpulse = penetrationImpulse+velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd->m_appliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + btScalar oldNormalImpulse = cpd->m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; @@ -145,13 +150,15 @@ float resolveSingleCollision( } -float resolveSingleFriction( +btScalar resolveSingleFriction( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { + (void)solverInfo; + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); const btVector3& pos2 = contactPoint.getPositionWorldOnB(); @@ -161,11 +168,11 @@ float resolveSingleFriction( btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; btScalar limit = cpd->m_appliedImpulse * combinedFriction; - if (cpd->m_appliedImpulse>0.f) + if (cpd->m_appliedImpulse>btScalar(0.)) //friction { //apply friction in the 2 tangential directions @@ -183,7 +190,7 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j1 = -vrel * cpd->m_jacDiagABInvTangent0; - float oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; + btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1; GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit); GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit); @@ -197,7 +204,7 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j2 = -vrel * cpd->m_jacDiagABInvTangent1; - float oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; + btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2; GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit); GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit); @@ -226,13 +233,15 @@ float resolveSingleFriction( } -float resolveSingleFrictionOriginal( +btScalar resolveSingleFrictionOriginal( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { + (void)solverInfo; + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); const btVector3& pos2 = contactPoint.getPositionWorldOnB(); @@ -242,10 +251,10 @@ float resolveSingleFrictionOriginal( btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; btScalar limit = cpd->m_appliedImpulse * combinedFriction; - //if (contactPoint.m_appliedImpulse>0.f) + //if (contactPoint.m_appliedImpulse>btScalar(0.)) //friction { //apply friction in the 2 tangential directions @@ -260,7 +269,7 @@ float resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent0; - float total = cpd->m_accumulatedTangentImpulse0 + j; + btScalar total = cpd->m_accumulatedTangentImpulse0 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); j = total - cpd->m_accumulatedTangentImpulse0; @@ -280,7 +289,7 @@ float resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent1; - float total = cpd->m_accumulatedTangentImpulse1 + j; + btScalar total = cpd->m_accumulatedTangentImpulse1 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); j = total - cpd->m_accumulatedTangentImpulse1; @@ -295,7 +304,7 @@ float resolveSingleFrictionOriginal( //velocity + friction //response between two dynamic objects with friction -float resolveSingleCollisionCombined( +btScalar resolveSingleCollisionCombined( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, @@ -315,11 +324,11 @@ float resolveSingleCollisionCombined( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar Kfps = 1.f / solverInfo.m_timeStep ; + btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - //float damping = solverInfo.m_damping ; - float Kerp = solverInfo.m_erp; - float Kcor = Kerp *Kfps; + //btScalar damping = solverInfo.m_damping ; + btScalar Kerp = solverInfo.m_erp; + btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); @@ -334,9 +343,9 @@ float resolveSingleCollisionCombined( btScalar normalImpulse = penetrationImpulse+velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd->m_appliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + btScalar oldNormalImpulse = cpd->m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; @@ -367,7 +376,7 @@ float resolveSingleCollisionCombined( btVector3 lat_vel = vel - normal * rel_vel; btScalar lat_rel_vel = lat_vel.length(); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; if (cpd->m_appliedImpulse > 0) if (lat_rel_vel > SIMD_EPSILON) @@ -390,12 +399,19 @@ float resolveSingleCollisionCombined( return normalImpulse; } -float resolveSingleFrictionEmpty( + +btScalar resolveSingleFrictionEmpty( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { - return 0.f; + (void)contactPoint; + (void)body1; + (void)body2; + (void)solverInfo; + + + return btScalar(0.); }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index d88ba0d8ed4..0834deddeac 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -19,8 +19,8 @@ subject to the following restrictions: //todo: make into a proper class working with the iterative constraint solver class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btScalar.h" struct btContactSolverInfo; class btManifoldPoint; @@ -33,7 +33,7 @@ enum { }; -typedef float (*ContactSolverFunc)(btRigidBody& body1, +typedef btScalar (*ContactSolverFunc)(btRigidBody& body1, btRigidBody& body2, class btManifoldPoint& contactPoint, const btContactSolverInfo& info); @@ -42,15 +42,15 @@ typedef float (*ContactSolverFunc)(btRigidBody& body1, struct btConstraintPersistentData { inline btConstraintPersistentData() - :m_appliedImpulse(0.f), - m_prevAppliedImpulse(0.f), - m_accumulatedTangentImpulse0(0.f), - m_accumulatedTangentImpulse1(0.f), - m_jacDiagABInv(0.f), + :m_appliedImpulse(btScalar(0.)), + m_prevAppliedImpulse(btScalar(0.)), + m_accumulatedTangentImpulse0(btScalar(0.)), + m_accumulatedTangentImpulse1(btScalar(0.)), + m_jacDiagABInv(btScalar(0.)), m_persistentLifeTime(0), - m_restitution(0.f), - m_friction(0.f), - m_penetration(0.f), + m_restitution(btScalar(0.)), + m_friction(btScalar(0.)), + m_penetration(btScalar(0.)), m_contactSolverFunc(0), m_frictionSolverFunc(0) { @@ -58,18 +58,18 @@ struct btConstraintPersistentData /// total applied impulse during most recent frame - float m_appliedImpulse; - float m_prevAppliedImpulse; - float m_accumulatedTangentImpulse0; - float m_accumulatedTangentImpulse1; + btScalar m_appliedImpulse; + btScalar m_prevAppliedImpulse; + btScalar m_accumulatedTangentImpulse0; + btScalar m_accumulatedTangentImpulse1; - float m_jacDiagABInv; - float m_jacDiagABInvTangent0; - float m_jacDiagABInvTangent1; + btScalar m_jacDiagABInv; + btScalar m_jacDiagABInvTangent0; + btScalar m_jacDiagABInvTangent1; int m_persistentLifeTime; - float m_restitution; - float m_friction; - float m_penetration; + btScalar m_restitution; + btScalar m_friction; + btScalar m_penetration; btVector3 m_frictionWorldTangential0; btVector3 m_frictionWorldTangential1; @@ -91,19 +91,28 @@ struct btConstraintPersistentData ///positive distance = separation, negative distance = penetration void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep); + btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); ///contact constraint resolution: ///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint ///positive distance = separation, negative distance = penetration -float resolveSingleCollision( +btScalar resolveSingleCollision( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& info); -float resolveSingleFriction( +btScalar resolveSingleFriction( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + ); + + + +btScalar resolveSingleCollisionCombined( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index ed1ba6ac1ba..c3c73e300f4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -22,25 +22,25 @@ struct btContactSolverInfo inline btContactSolverInfo() { - m_tau = 0.6f; - m_damping = 1.0f; - m_friction = 0.3f; - m_restitution = 0.f; - m_maxErrorReduction = 20.f; + m_tau = btScalar(0.6); + m_damping = btScalar(1.0); + m_friction = btScalar(0.3); + m_restitution = btScalar(0.); + m_maxErrorReduction = btScalar(20.); m_numIterations = 10; - m_erp = 0.4f; - m_sor = 1.3f; + m_erp = btScalar(0.4); + m_sor = btScalar(1.3); } - float m_tau; - float m_damping; - float m_friction; - float m_timeStep; - float m_restitution; + btScalar m_tau; + btScalar m_damping; + btScalar m_friction; + btScalar m_timeStep; + btScalar m_restitution; int m_numIterations; - float m_maxErrorReduction; - float m_sor; - float m_erp; + btScalar m_maxErrorReduction; + btScalar m_sor; + btScalar m_erp; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index b2132a8d4f3..747d10d1f8b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -17,8 +17,9 @@ subject to the following restrictions: #include "btGeneric6DofConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" +#include <new> -static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f }; +static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) }; static const int kAxisA[] = { 1, 0, 0 }; static const int kAxisB[] = { 2, 2, 1 }; #define GENERIC_D6_DISABLE_WARMSTARTING 1 @@ -38,9 +39,9 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& //so start all locked for (int i=0; i<6;++i) { - m_lowerLimit[i] = 0.0f; - m_upperLimit[i] = 0.0f; - m_accumulatedImpulse[i] = 0.0f; + m_lowerLimit[i] = btScalar(0.0); + m_upperLimit[i] = btScalar(0.0); + m_accumulatedImpulse[i] = btScalar(0.0); } } @@ -83,7 +84,7 @@ void btGeneric6DofConstraint::buildJacobian() //optionally disable warmstarting #ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i] = 0.f; + m_accumulatedImpulse[i] = btScalar(0.); #endif //GENERIC_D6_DISABLE_WARMSTARTING // Apply accumulated impulse @@ -115,7 +116,7 @@ void btGeneric6DofConstraint::buildJacobian() m_rbB.getInvInertiaDiagLocal()); #ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i + 3] = 0.f; + m_accumulatedImpulse[i + 3] = btScalar(0.); #endif //GENERIC_D6_DISABLE_WARMSTARTING // Apply accumulated impulse @@ -127,7 +128,7 @@ void btGeneric6DofConstraint::buildJacobian() } } -float getMatrixElem(const btMatrix3x3& mat,int index) +btScalar getMatrixElem(const btMatrix3x3& mat,int index) { int row = index%3; int col = index / 3; @@ -143,9 +144,9 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) /// 0..8 - if (getMatrixElem(mat,2) < 1.0f) + if (getMatrixElem(mat,2) < btScalar(1.0)) { - if (getMatrixElem(mat,2) > -1.0f) + if (getMatrixElem(mat,2) > btScalar(-1.0)) { xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8)); xyz[1] = btAsin(getMatrixElem(mat,2)); @@ -157,7 +158,7 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) // WARNING. Not unique. XA - ZA = -atan2(r10,r11) xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); xyz[1] = -SIMD_HALF_PI; - xyz[2] = 0.0f; + xyz[2] = btScalar(0.0); return false; } } @@ -167,16 +168,17 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); xyz[1] = SIMD_HALF_PI; xyz[2] = 0.0; - return false; + } - return false; + + return false; } void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) { - btScalar tau = 0.1f; - btScalar damping = 1.0f; + btScalar tau = btScalar(0.1); + btScalar damping = btScalar(1.0); btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); @@ -199,7 +201,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) localNormalInA[i] = 1; btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); //velocity error (first order error) btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, @@ -207,8 +209,8 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); - btScalar lo = -1e30f; - btScalar hi = 1e30f; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); //handle the limits if (m_lowerLimit[i] < m_upperLimit[i]) @@ -217,14 +219,14 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) if (depth > m_upperLimit[i]) { depth -= m_upperLimit[i]; - lo = 0.f; + lo = btScalar(0.); } else { if (depth < m_lowerLimit[i]) { depth -= m_lowerLimit[i]; - hi = 0.f; + hi = btScalar(0.); } else { continue; @@ -234,9 +236,9 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) } btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; - float oldNormalImpulse = m_accumulatedImpulse[i]; - float sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[i] = sum > hi ? 0.f : sum < lo ? 0.f : sum; + btScalar oldNormalImpulse = m_accumulatedImpulse[i]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse; btVector3 impulse_vector = normalWorld * normalImpulse; @@ -267,7 +269,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - btScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); //velocity error (first order error) btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, @@ -279,27 +281,27 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btScalar rel_pos = kSign[i] * axisA.dot(axisB); - btScalar lo = -1e30f; - btScalar hi = 1e30f; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); //handle the twist limit if (m_lowerLimit[i+3] < m_upperLimit[i+3]) { //clamp the values - btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f; - btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f; + btScalar loLimit = m_lowerLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : btScalar(-1e30); + btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : btScalar(1e30); - float projAngle = -2.f*xyz[i]; + btScalar projAngle = btScalar(-1.)*xyz[i]; if (projAngle < loLimit) { - hi = 0.f; + hi = btScalar(0.); rel_pos = (loLimit - projAngle); } else { if (projAngle > hiLimit) { - lo = 0.f; + lo = btScalar(0.); rel_pos = (hiLimit - projAngle); } else { @@ -311,9 +313,9 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) //impulse btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; - float oldNormalImpulse = m_accumulatedImpulse[i+3]; - float sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[i+3] = sum > hi ? 0.f : sum < lo ? 0.f : sum; + btScalar oldNormalImpulse = m_accumulatedImpulse[i+3]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i+3] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse; // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) @@ -328,12 +330,13 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } btScalar btGeneric6DofConstraint::computeAngle(int axis) const { - btScalar angle; + btScalar angle = btScalar(0.f); switch (axis) { @@ -375,9 +378,12 @@ btScalar btGeneric6DofConstraint::computeAngle(int axis) const angle = btAtan2( s, c ); } break; - default: assert ( 0 ) ; break ; + default: + btAssert ( 0 ) ; + + break ; } - return angle; + return angle; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 329048b5737..b114e54fa69 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef GENERIC_6DOF_CONSTRAINT_H #define GENERIC_6DOF_CONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -41,12 +40,19 @@ class btGeneric6DofConstraint : public btTypedConstraint btScalar m_accumulatedImpulse[6]; + btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } public: btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ); btGeneric6DofConstraint(); + virtual void buildJacobian(); virtual void solveConstraint(btScalar timeStep); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index f72278e2cbf..27e30987549 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "btHingeConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - +#include <new> btHingeConstraint::btHingeConstraint(): m_enableAngularMotor(false) @@ -49,7 +49,7 @@ m_enableAngularMotor(false) void btHingeConstraint::buildJacobian() { - m_appliedImpulse = 0.f; + m_appliedImpulse = btScalar(0.); btVector3 normal(0,0,0); @@ -115,8 +115,8 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; btVector3 normal(0,0,0); - btScalar tau = 0.3f; - btScalar damping = 1.f; + btScalar tau = btScalar(0.3); + btScalar damping = btScalar(1.); //linear part if (!m_angularOnly) @@ -124,7 +124,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) for (int i=0;i<3;i++) { normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); @@ -165,27 +165,27 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btVector3 velrelOrthog = angAorthog-angBorthog; { //solve orthogonal angular velocity correction - float relaxation = 1.f; - float len = velrelOrthog.length(); - if (len > 0.00001f) + btScalar relaxation = btScalar(1.); + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); - float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation //todo: expose this 0.9 factor to developer - velrelOrthog *= (1.f/denom) * 0.9f; + velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9); } //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); - float len2 = angularError.length(); - if (len2>0.00001f) + btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); - float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (1.f/denom2) * relaxation; + angularError *= (btScalar(1.)/denom2) * relaxation; } m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); @@ -204,10 +204,10 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; - float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + + btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA); - btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;; + btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;; //todo: should clip against accumulated impulse btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; @@ -223,6 +223,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) void btHingeConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 553ec135c8a..5c1ceafbc5b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef HINGECONSTRAINT_H #define HINGECONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -38,8 +37,8 @@ class btHingeConstraint : public btTypedConstraint bool m_angularOnly; - float m_motorTargetVelocity; - float m_maxMotorImpulse; + btScalar m_motorTargetVelocity; + btScalar m_maxMotorImpulse; bool m_enableAngularMotor; public: @@ -70,7 +69,7 @@ public: m_angularOnly = angularOnly; } - void enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse) + void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) { m_enableAngularMotor = enableMotor; m_motorTargetVelocity = targetVelocity; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index 384e4f7bab5..aae3ed0373f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef JACOBIAN_ENTRY_H #define JACOBIAN_ENTRY_H -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "../../LinearMath/btVector3.h" +#include "../Dynamics/btRigidBody.h" //notes: @@ -50,7 +50,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //angular constraint between two different rigidbodies @@ -59,7 +59,7 @@ public: const btMatrix3x3& world2B, const btVector3& inertiaInvA, const btVector3& inertiaInvB) - :m_linearJointAxis(btVector3(0.f,0.f,0.f)) + :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) { m_aJ= world2A*jointAxis; m_bJ = world2B*-jointAxis; @@ -67,7 +67,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //angular constraint between two different rigidbodies @@ -75,7 +75,7 @@ public: const btVector3& axisInB, const btVector3& inertiaInvA, const btVector3& inertiaInvB) - : m_linearJointAxis(btVector3(0.f,0.f,0.f)) + : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) , m_aJ(axisInA) , m_bJ(-axisInB) { @@ -83,7 +83,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //constraint on one rigidbody @@ -98,10 +98,10 @@ public: m_aJ= world2A*(rel_pos1.cross(jointAxis)); m_bJ = world2A*(rel_pos2.cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = btVector3(0.f,0.f,0.f); + m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } btScalar getDiagonal() const { return m_Adiag; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index d15bdaad790..aacb0a3ea66 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btPoint2PointConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" - +#include <new> @@ -39,7 +39,7 @@ btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector void btPoint2PointConstraint::buildJacobian() { - m_appliedImpulse = 0.f; + m_appliedImpulse = btScalar(0.); btVector3 normal(0,0,0); @@ -76,7 +76,7 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep) for (int i=0;i<3;i++) { normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); @@ -110,6 +110,7 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep) void btPoint2PointConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 8aae8d74ce7..71da8ac0347 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef POINT2POINTCONSTRAINT_H #define POINT2POINTCONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -26,12 +25,12 @@ class btRigidBody; struct btConstraintSetting { btConstraintSetting() : - m_tau(0.3f), - m_damping(1.f) + m_tau(btScalar(0.3)), + m_damping(btScalar(1.)) { } - float m_tau; - float m_damping; + btScalar m_tau; + btScalar m_damping; }; /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index eaf172b9395..4366284ea73 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -24,7 +24,13 @@ subject to the following restrictions: #include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include <new> +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btQuickprof.h" +#include "btSolverBody.h" +#include "btSolverConstraint.h" +#include "LinearMath/btAlignedObjectArray.h" #ifdef USE_PROFILE #include "LinearMath/btQuickprof.h" @@ -36,24 +42,26 @@ int gTotalContactPoints = 0; struct btOrderIndex { - short int m_manifoldIndex; - short int m_pointIndex; + int m_manifoldIndex; + int m_pointIndex; }; + + #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384 static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; -static unsigned long btSeed2 = 0; -unsigned long btRand2() + + +unsigned long btSequentialImpulseConstraintSolver::btRand2() { - btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff; - return btSeed2; + m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + return m_btSeed2; } - //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btRandInt2 (int n) +int btSequentialImpulseConstraintSolver::btRandInt2 (int n) { // seems good; xor-fold and modulus const unsigned long un = n; @@ -82,15 +90,7 @@ int btRandInt2 (int n) -int btRandIntWrong (int n) -{ - float a = float(n) / 4294967296.0f; -// printf("n = %d\n",n); -// printf("a = %f\n",a); - int res = (int) (float(btRand2()) * a); -// printf("res=%d\n",res); - return res; -} + bool MyContactDestroyedCallback(void* userPersistentData) { @@ -102,18 +102,12 @@ bool MyContactDestroyedCallback(void* userPersistentData) return true; } -btSequentialImpulseConstraintSolver3::btSequentialImpulseConstraintSolver3() -{ - btSeed2 = 0; - setSolverMode(SOLVER_RANDMIZE_ORDER); -} btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_solverMode(SOLVER_USE_WARMSTARTING) +:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY), //not using SOLVER_USE_WARMSTARTING, +m_btSeed2(0) { - btSeed2 = 0; - gContactDestroyedCallback = &MyContactDestroyedCallback; //initialize default friction/contact funcs @@ -127,35 +121,487 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() } } -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) + +void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody) +{ +/* int size = sizeof(btSolverBody); + int sizeofrb = sizeof(btRigidBody); + int sizemanifold = sizeof(btPersistentManifold); + int sizeofmp = sizeof(btManifoldPoint); + int sizeofPersistData = sizeof (btConstraintPersistentData); +*/ + + solverBody->m_angularVelocity = rigidbody->getAngularVelocity(); + solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition(); + solverBody->m_friction = rigidbody->getFriction(); +// solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld(); + solverBody->m_invMass = rigidbody->getInvMass(); + solverBody->m_linearVelocity = rigidbody->getLinearVelocity(); + solverBody->m_originalBody = rigidbody; + solverBody->m_angularFactor = rigidbody->getAngularFactor(); +} + +btScalar penetrationResolveFactor = btScalar(0.9); +btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +{ + btScalar rest = restitution * -rel_vel; + return rest; +} + + + + + + +//velocity + friction +//response between two dynamic objects with friction +SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + (void)solverInfo; + + btScalar normalImpulse(0.f); + { + if (contactConstraint.m_penetration < 0.f) + return 0.f; + + // Optimized version of projected relative velocity, use precomputed cross products with normal + // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + // btVector3 vel = vel1 - vel2; + // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); + + btScalar rel_vel; + btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + + rel_vel = vel1Dotn-vel2Dotn; + + + btScalar positionalError = contactConstraint.m_penetration; + btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; + + btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; + + btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse; + btScalar velocitySum = oldVelocityImpulse + velocityImpulse; + contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum; + + normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, + contactConstraint.m_angularComponentA,normalImpulse); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, + contactConstraint.m_angularComponentB,-normalImpulse); + } + + } + + + + return normalImpulse; +} + + +#ifndef NO_FRICTION_TANGENTIALS + +SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo, + btScalar appliedNormalImpulse) { + (void)solverInfo; + - btContactSolverInfo info = infoGlobal; + const btScalar combinedFriction = contactConstraint.m_friction; + + const btScalar limit = appliedNormalImpulse * combinedFriction; + + if (appliedNormalImpulse>btScalar(0.)) + //friction + { + + btScalar j1; + { - int numiter = infoGlobal.m_numIterations; -#ifdef USE_PROFILE - btProfiler::beginBlock("solve"); -#endif //USE_PROFILE + btScalar rel_vel; + const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + rel_vel = vel1Dotn-vel2Dotn; + + // calculate j that moves us to zero relative velocity + j1 = -rel_vel * contactConstraint.m_jacDiagABInv; + btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; + contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; + GEN_set_min(contactConstraint.m_appliedImpulse, limit); + GEN_set_max(contactConstraint.m_appliedImpulse, -limit); + j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; - int totalPoints = 0; + } + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); + } + + } + return 0.f; +} +#else + +//velocity + friction +//response between two dynamic objects with friction +btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + + btVector3 vel1; + btVector3 vel2; + btScalar normalImpulse(0.f); + { - int j; - for (j=0;j<numManifolds;j++) + const btVector3& normal = contactConstraint.m_contactNormal; + if (contactConstraint.m_penetration < 0.f) + return 0.f; + + + body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + btVector3 lat_vel = vel - normal * rel_vel; + btScalar lat_rel_vel = lat_vel.length2(); + + btScalar combinedFriction = contactConstraint.m_friction; + const btVector3& rel_pos1 = contactConstraint.m_rel_posA; + const btVector3& rel_pos2 = contactConstraint.m_rel_posB; + + + //if (contactConstraint.m_appliedVelocityImpulse > 0.f) + if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) { - btPersistentManifold* manifold = manifoldPtr[j]; - prepareConstraints(manifold,info,debugDrawer); + lat_rel_vel = btSqrt(lat_rel_vel); + + lat_vel /= lat_rel_vel; + btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel); + btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel); + btScalar friction_impulse = lat_rel_vel / + (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); + btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction; + + GEN_set_min(friction_impulse, normal_impulse); + GEN_set_max(friction_impulse, -normal_impulse); + body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); + body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); + } + } + + return normalImpulse; +} + +#endif //NO_FRICTION_TANGENTIALS + +btAlignedObjectArray<btSolverBody> tmpSolverBodyPool; +btAlignedObjectArray<btSolverConstraint> tmpSolverConstraintPool; +btAlignedObjectArray<btSolverConstraint> tmpSolverFrictionConstraintPool; + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + (void)stackAlloc; + (void)debugDrawer; + + if (!(numConstraints + numManifolds)) + { +// printf("empty\n"); + return 0.f; + } + + BEGIN_PROFILE("refreshManifolds"); + + int i; + for (i=0;i<numManifolds;i++) + { + btPersistentManifold* manifold = manifoldPtr[i]; + btRigidBody* rb0 = (btRigidBody*)manifold->getBody0(); + btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); - for (int p=0;p<manifoldPtr[j]->getNumContacts();p++) + manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + + } + + END_PROFILE("refreshManifolds"); + + + BEGIN_PROFILE("gatherSolverData"); + + //int sizeofSB = sizeof(btSolverBody); + //int sizeofSC = sizeof(btSolverConstraint); + + + //if (1) + { + //if m_stackAlloc, try to pack bodies/constraints to speed up solving +// btBlock* sablock; +// sablock = stackAlloc->beginBlock(); + + // int memsize = 16; +// unsigned char* stackMemory = stackAlloc->allocate(memsize); + + + //todo: use stack allocator for this temp memory + int minReservation = numManifolds*2; + + tmpSolverBodyPool.reserve(minReservation); + + { + for (int i=0;i<numBodies;i++) { - gOrder[totalPoints].m_manifoldIndex = j; - gOrder[totalPoints].m_pointIndex = p; - totalPoints++; + btRigidBody* rb = btRigidBody::upcast(bodies[i]); + if (rb && (rb->getIslandTag() >= 0)) + { + btAssert(rb->getCompanionId() < 0); + int solverBodyId = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb); + rb->setCompanionId(solverBodyId); + } + } + } + + + tmpSolverConstraintPool.reserve(minReservation); + tmpSolverFrictionConstraintPool.reserve(minReservation); + { + int i; + + for (i=0;i<numManifolds;i++) + { + btPersistentManifold* manifold = manifoldPtr[i]; + btRigidBody* rb0 = (btRigidBody*)manifold->getBody0(); + btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); + + + int solverBodyIdA=-1; + int solverBodyIdB=-1; + + if (manifold->getNumContacts()) + { + + + + if (rb0->getIslandTag() >= 0) + { + solverBodyIdA = rb0->getCompanionId(); + } else + { + //create a static body + solverBodyIdA = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb0); + } + + if (rb1->getIslandTag() >= 0) + { + solverBodyIdB = rb1->getCompanionId(); + } else + { + //create a static body + solverBodyIdB = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb1); + } + } + + for (int j=0;j<manifold->getNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + int frictionIndex = tmpSolverConstraintPool.size(); + + if (cp.getDistance() <= btScalar(0.)) + { + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); + + + btScalar relaxation = 1.f; + + { + btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand(); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; + + + + { + //can be optimized, the cross products are already calculated + btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + } + + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); + + + btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); + solverConstraint.m_friction = cp.m_combinedFriction; + btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (rest <= btScalar(0.)) + { + rest = 0.f; + }; + + btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; + if (rest > penVel) + { + rest = btScalar(0.); + } + solverConstraint.m_restitution = rest; + + solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep); + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedVelocityImpulse = 0.f; + + + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; + } + + //create 2 '1d axis' constraints for 2 tangential friction directions + + //re-calculate friction direction every frame, todo: check if this is really needed + btVector3 frictionTangential0a, frictionTangential1b; + + btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); + + { + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential0a; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + + { + btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; + } + { + btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; + } + + } + + + { + + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential1b; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + { + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + } + } + + } + } } } } + END_PROFILE("gatherSolverData"); + + BEGIN_PROFILE("prepareConstraints"); + + btContactSolverInfo info = infoGlobal; { int j; @@ -166,21 +612,57 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma } } - //should traverse the contacts random order... - int iteration; + btAlignedObjectArray<int> gOrderTmpConstraintPool; + btAlignedObjectArray<int> gOrderFrictionConstraintPool; + int numConstraintPool = tmpSolverConstraintPool.size(); + int numFrictionPool = tmpSolverFrictionConstraintPool.size(); + + ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + gOrderTmpConstraintPool.resize(numConstraintPool); + gOrderFrictionConstraintPool.resize(numFrictionPool); { - for ( iteration = 0;iteration<numiter-1;iteration++) + int i; + for (i=0;i<numConstraintPool;i++) + { + gOrderTmpConstraintPool[i] = i; + } + for (i=0;i<numFrictionPool;i++) { + gOrderFrictionConstraintPool[i] = i; + } + } + + + + + END_PROFILE("prepareConstraints"); + + + BEGIN_PROFILE("solveConstraints"); + + //should traverse the contacts random order... + int iteration; + { + for ( iteration = 0;iteration<info.m_numIterations;iteration++) + { + int j; if (m_solverMode & SOLVER_RANDMIZE_ORDER) { if ((iteration & 7) == 0) { - for (j=0; j<totalPoints; ++j) { - btOrderIndex tmp = gOrder[j]; + for (j=0; j<numConstraintPool; ++j) { + int tmp = gOrderTmpConstraintPool[j]; int swapi = btRandInt2(j+1); - gOrder[j] = gOrder[swapi]; - gOrder[swapi] = tmp; + gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; + gOrderTmpConstraintPool[swapi] = tmp; + } + + for (j=0; j<numFrictionPool; ++j) { + int tmp = gOrderFrictionConstraintPool[j]; + int swapi = btRandInt2(j+1); + gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; + gOrderFrictionConstraintPool[swapi] = tmp; } } } @@ -188,38 +670,98 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma for (j=0;j<numConstraints;j++) { btTypedConstraint* constraint = constraints[j]; + ///todo: use solver bodies, so we don't need to copy from/to btRigidBody + + if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity(); + } + if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity(); + } + constraint->solveConstraint(info.m_timeStep); + + if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity(); + } + if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); + } + } - for (j=0;j<totalPoints;j++) { - btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; - solve( (btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + int numPoolConstraints = tmpSolverConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; + resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info); + } } - - for (j=0;j<totalPoints;j++) + { - btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; - solveFriction((btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) + { + btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; + btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse); + } } + + + } } -#ifdef USE_PROFILE - btProfiler::endBlock("solve"); -#endif //USE_PROFILE + for ( i=0;i<tmpSolverBodyPool.size();i++) + { + tmpSolverBodyPool[i].writebackVelocity(); + } + + END_PROFILE("solveConstraints"); + +// printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size()); + +/* + printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size()); + printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size()); + printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size()); + + + printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity()); + printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity()); + printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity()); +*/ + + tmpSolverBodyPool.resize(0); + tmpSolverConstraintPool.resize(0); + tmpSolverFrictionConstraintPool.resize(0); + return 0.f; } - /// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { + + if (getSolverMode() & SOLVER_CACHE_FRIENDLY) + { + return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + } + + + BEGIN_PROFILE("prepareConstraints"); + btContactSolverInfo info = infoGlobal; int numiter = infoGlobal.m_numIterations; @@ -227,21 +769,25 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man btProfiler::beginBlock("solve"); #endif //USE_PROFILE + int totalPoints = 0; + + { - int j; + short j; for (j=0;j<numManifolds;j++) { btPersistentManifold* manifold = manifoldPtr[j]; prepareConstraints(manifold,info,debugDrawer); - for (int p=0;p<manifoldPtr[j]->getNumContacts();p++) + + for (short p=0;p<manifoldPtr[j]->getNumContacts();p++) { - //interleaving here gives better results - solve( (btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1() - ,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer); + gOrder[totalPoints].m_manifoldIndex = j; + gOrder[totalPoints].m_pointIndex = p; + totalPoints++; } } } + { int j; for (j=0;j<numConstraints;j++) @@ -251,66 +797,77 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man } } + END_PROFILE("prepareConstraints"); + + + BEGIN_PROFILE("solveConstraints"); + //should traverse the contacts random order... int iteration; - for ( iteration = 0;iteration<numiter-1;iteration++) { - int j; - - for (j=0;j<numConstraints;j++) + for ( iteration = 0;iteration<numiter;iteration++) { - btTypedConstraint* constraint = constraints[j]; - constraint->solveConstraint(info.m_timeStep); - } + int j; + if (m_solverMode & SOLVER_RANDMIZE_ORDER) + { + if ((iteration & 7) == 0) { + for (j=0; j<totalPoints; ++j) { + btOrderIndex tmp = gOrder[j]; + int swapi = btRandInt2(j+1); + gOrder[j] = gOrder[swapi]; + gOrder[swapi] = tmp; + } + } + } - for (j=0;j<numManifolds;j++) - { - btPersistentManifold* manifold = manifoldPtr[j]; - for (int p=0;p<manifold->getNumContacts();p++) + for (j=0;j<numConstraints;j++) { + btTypedConstraint* constraint = constraints[j]; + constraint->solveConstraint(info.m_timeStep); + } + + for (j=0;j<totalPoints;j++) + { + btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; solve( (btRigidBody*)manifold->getBody0(), (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(p),info,iteration,debugDrawer); + ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } - } - - } - - for ( iteration = 0;iteration<numiter-1;iteration++) - { - int j; - for (j=0;j<numManifolds;j++) - { - btPersistentManifold* manifold = manifoldPtr[j]; - for (int p=0;p<manifold->getNumContacts();p++) + + for (j=0;j<totalPoints;j++) { + btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; solveFriction((btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer); + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } } } - + END_PROFILE("solveConstraints"); + + #ifdef USE_PROFILE btProfiler::endBlock("solve"); #endif //USE_PROFILE - return 0.f; -} -float penetrationResolveFactor = 0.9f; -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) -{ - btScalar rest = restitution * -rel_vel; - return rest; + + return btScalar(0.); } + + + + + void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) { + (void)debugDrawer; + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); @@ -327,7 +884,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol for (int i=0;i<numpoints ;i++) { btManifoldPoint& cp = manifoldPtr->getContactPoint(i); - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -376,7 +933,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } assert(cpd); - cpd->m_jacDiagABInv = 1.f / jacDiagAB; + cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB; //Dependent on Rigidbody A and B types, fetch the contact/friction response func //perhaps do a similar thing for friction/restutution combiner funcs... @@ -390,14 +947,14 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol btScalar rel_vel; rel_vel = cp.m_normalWorldOnB.dot(vel); - float combinedRestitution = cp.m_combinedRestitution; + btScalar combinedRestitution = cp.m_combinedRestitution; - cpd->m_penetration = cp.getDistance(); + cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations); cpd->m_friction = cp.m_combinedFriction; cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); - if (cpd->m_restitution <= 0.) //0.f) + if (cpd->m_restitution <= btScalar(0.)) { - cpd->m_restitution = 0.0f; + cpd->m_restitution = btScalar(0.0); }; @@ -408,18 +965,18 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol if (cpd->m_restitution > penVel) { - cpd->m_penetration = 0.f; + cpd->m_penetration = btScalar(0.); } - float relaxation = info.m_damping; + btScalar relaxation = info.m_damping; if (m_solverMode & SOLVER_USE_WARMSTARTING) { cpd->m_appliedImpulse *= relaxation; } else { - cpd->m_appliedImpulse =0.f; + cpd->m_appliedImpulse =btScalar(0.); } //for friction @@ -432,12 +989,12 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol #define NO_FRICTION_WARMSTART 1 #ifdef NO_FRICTION_WARMSTART - cpd->m_accumulatedTangentImpulse0 = 0.f; - cpd->m_accumulatedTangentImpulse1 = 0.f; + cpd->m_accumulatedTangentImpulse0 = btScalar(0.); + cpd->m_accumulatedTangentImpulse1 = btScalar(0.); #endif //NO_FRICTION_WARMSTART - float denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); - float denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); - float denom = relaxation/(denom0+denom1); + btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); + btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); + btScalar denom = relaxation/(denom0+denom1); cpd->m_jacDiagABInvTangent0 = denom; @@ -492,16 +1049,54 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } } -float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) + +btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { + btScalar maxImpulse = btScalar(0.); + + { + + btVector3 color(0,1,0); + { + if (cp.getDistance() <= btScalar(0.)) + { - float maxImpulse = 0.f; + if (iter == 0) + { + if (debugDrawer) + debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + + { + + //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + btScalar impulse = resolveSingleCollisionCombined( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } + } + } + } + return maxImpulse; +} + + + +btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + + btScalar maxImpulse = btScalar(0.); { btVector3 color(0,1,0); { - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { if (iter == 0) @@ -513,7 +1108,7 @@ float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* { btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - float impulse = cpd->m_contactSolverFunc( + btScalar impulse = cpd->m_contactSolverFunc( *body0,*body1, cp, info); @@ -528,16 +1123,19 @@ float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* return maxImpulse; } -float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { + (void)debugDrawer; + (void)iter; + { btVector3 color(0,1,0); { - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; @@ -552,5 +1150,5 @@ float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRi } - return 0.f; + return btScalar(0.); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 0989a86e2cd..13e70c41be4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -18,7 +18,6 @@ subject to the following restrictions: #include "btConstraintSolver.h" class btIDebugDraw; - #include "btContactConstraint.h" @@ -31,8 +30,8 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; @@ -40,6 +39,8 @@ protected: //choose between several modes, different friction model etc. int m_solverMode; + ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction + unsigned long m_btSeed2; public: @@ -47,7 +48,8 @@ public: { SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, - SOLVER_USE_WARMSTARTING = 4 + SOLVER_USE_WARMSTARTING = 4, + SOLVER_CACHE_FRIENDLY = 8 }; btSequentialImpulseConstraintSolver(); @@ -68,7 +70,12 @@ public: virtual ~btSequentialImpulseConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + + virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + + btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + void setSolverMode(int mode) { @@ -79,20 +86,24 @@ public: { return m_solverMode; } -}; - -/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering -class btSequentialImpulseConstraintSolver3 : public btSequentialImpulseConstraintSolver -{ -public: - btSequentialImpulseConstraintSolver3(); + unsigned long btRand2(); - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + int btRandInt2 (int n); + void setRandSeed(unsigned long seed) + { + m_btSeed2 = seed; + } + unsigned long getRandSeed() const + { + return m_btSeed2; + } }; + + #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp index edca6c8fa08..0c7dbd668bb 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -43,12 +43,18 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1) { + (void)linvelA; + (void)linvelB; + (void)angvelB; + (void)angvelA; - imp0 = 0.f; - imp1 = 0.f; - btScalar len = fabs(normalA.length())-1.f; - if (fabs(len) >= SIMD_EPSILON) + + imp0 = btScalar(0.); + imp1 = btScalar(0.); + + btScalar len = btFabs(normalA.length()) - btScalar(1.); + if (btFabs(len) >= SIMD_EPSILON) return; btAssert(len < SIMD_EPSILON); @@ -67,7 +73,7 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv - btScalar massTerm = 1.f / (invMassA + invMassB); + btScalar massTerm = btScalar(1.) / (invMassA + invMassB); // calculate rhs (or error) terms @@ -87,7 +93,7 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; @@ -126,11 +132,18 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btScalar& imp0,btScalar& imp1) { - imp0 = 0.f; - imp1 = 0.f; + (void)linvelA; + (void)linvelB; + (void)angvelA; + (void)angvelB; + + - btScalar len = fabs(normalA.length())-1.f; - if (fabs(len) >= SIMD_EPSILON) + imp0 = btScalar(0.); + imp1 = btScalar(0.); + + btScalar len = btFabs(normalA.length()) - btScalar(1.); + if (btFabs(len) >= SIMD_EPSILON) return; btAssert(len < SIMD_EPSILON); @@ -164,7 +177,7 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; @@ -178,41 +191,41 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] - if ( imp0 > 0.0f) + if ( imp0 > btScalar(0.0)) { - if ( imp1 > 0.0f ) + if ( imp1 > btScalar(0.0) ) { //both positive } else { - imp1 = 0.f; + imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > 0.0f ) + if ( imp0 > btScalar(0.0) ) { } else { - imp0 = 0.f; + imp0 = btScalar(0.); } } } else { - imp0 = 0.f; + imp0 = btScalar(0.); imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= 0.0f ) + if ( imp1 <= btScalar(0.0) ) { - imp1 = 0.f; + imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > 0.0f ) + if ( imp0 > btScalar(0.0) ) { } else { - imp0 = 0.f; + imp0 = btScalar(0.); } } else { @@ -221,7 +234,7 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( } - +/* void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, @@ -238,4 +251,5 @@ void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invI { } +*/ diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h index 639e4c9433f..e7d26645c6a 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef SOLVE_2LINEAR_CONSTRAINT_H #define SOLVE_2LINEAR_CONSTRAINT_H -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btVector3.h" class btRigidBody; @@ -85,7 +85,7 @@ public: btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1); - +/* void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, @@ -100,6 +100,7 @@ public: btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1); +*/ }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h new file mode 100644 index 00000000000..0ab536f42b3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -0,0 +1,71 @@ +/* +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_SOLVER_BODY_H +#define BT_SOLVER_BODY_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + + + + +ATTRIBUTE_ALIGNED16 (struct) btSolverBody +{ + btVector3 m_centerOfMassPosition; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btRigidBody* m_originalBody; + float m_invMass; + float m_friction; + float m_angularFactor; + + inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + m_linearVelocity += linearComponent*impulseMagnitude; + m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor; + } + + void writebackVelocity() + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity); + } + } + + void readVelocity() + { + if (m_invMass) + { + m_linearVelocity = m_originalBody->getLinearVelocity(); + m_angularVelocity = m_originalBody->getAngularVelocity(); + } + } + + + + +}; + +#endif //BT_SOLVER_BODY_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h new file mode 100644 index 00000000000..f1f40ffdf19 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -0,0 +1,63 @@ + + +/* +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_SOLVER_CONSTRAINT_H +#define BT_SOLVER_CONSTRAINT_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + +//#define NO_FRICTION_TANGENTIALS 1 + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint +{ + btVector3 m_relpos1CrossNormal; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal; + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + btScalar m_appliedVelocityImpulse; + int m_solverBodyIdA; + int m_solverBodyIdB; + btScalar m_friction; + btScalar m_restitution; + btScalar m_jacDiagABInv; + btScalar m_penetration; + btScalar m_appliedImpulse; + + int m_constraintType; + int m_frictionIndex; + int m_unusedPadding[2]; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + + + + + + +#endif //BT_SOLVER_CONSTRAINT_H + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index cb3fa72d440..a15b3e026cd 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -24,18 +24,18 @@ btTypedConstraint::btTypedConstraint() m_userConstraintId(-1), m_rbA(s_fixed), m_rbB(s_fixed), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } btTypedConstraint::btTypedConstraint(btRigidBody& rbA) : m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(rbA), m_rbB(s_fixed), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } @@ -45,9 +45,9 @@ btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB) m_userConstraintId(-1), m_rbA(rbA), m_rbB(rbB), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index bc25eaa759c..dfee6e80d0e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -17,19 +17,25 @@ subject to the following restrictions: #define TYPED_CONSTRAINT_H class btRigidBody; -#include "LinearMath/btScalar.h" +#include "../../LinearMath/btScalar.h" ///TypedConstraint is the baseclass for Bullet constraints and vehicles class btTypedConstraint { int m_userConstraintType; int m_userConstraintId; - + + btTypedConstraint& operator=(btTypedConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } protected: btRigidBody& m_rbA; btRigidBody& m_rbB; - float m_appliedImpulse; + btScalar m_appliedImpulse; public: @@ -81,7 +87,7 @@ public: { return m_userConstraintId; } - float getAppliedImpulse() + btScalar getAppliedImpulse() { return m_appliedImpulse; } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index fc37cf2e3c3..f80a0826fbf 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -16,7 +16,6 @@ subject to the following restrictions: #include "btDiscreteDynamicsWorld.h" - //collision detection #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" @@ -54,8 +53,6 @@ subject to the following restrictions: -#include <algorithm> - btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) @@ -63,7 +60,7 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOver m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), m_debugDrawer(0), m_gravity(0,-10,0), -m_localTime(1.f/60.f), +m_localTime(btScalar(1.)/btScalar(60.)), m_profileTimings(0) { m_islandManager = new btSimulationIslandManager(); @@ -81,10 +78,10 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() delete m_constraintSolver; } -void btDiscreteDynamicsWorld::saveKinematicState(float timeStep) +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) { - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for (int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -110,27 +107,27 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() { //todo: iterate over awake simulation islands! - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) { - btVector3 color(255.f,255.f,255.f); + btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); switch(colObj->getActivationState()) { case ACTIVE_TAG: - color = btVector3(255.f,255.f,255.f); break; + color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; case ISLAND_SLEEPING: - color = btVector3(0.f,255.f,0.f);break; + color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; case WANTS_DEACTIVATION: - color = btVector3(0.f,255.f,255.f);break; + color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; case DISABLE_DEACTIVATION: - color = btVector3(255.f,0.f,0.f);break; + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; case DISABLE_SIMULATION: - color = btVector3(255.f,255.f,0.f);break; + color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; default: { - color = btVector3(255.f,0.f,0.f); + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); } }; @@ -139,7 +136,10 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() btRigidBody* body = btRigidBody::upcast(colObj); if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) { - if (body->getActivationState() != ISLAND_SLEEPING) + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + //so todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) { btTransform interpolatedTransform; btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), @@ -152,7 +152,7 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) { - for (unsigned int i=0;i<this->m_vehicles.size();i++) + for ( int i=0;i<this->m_vehicles.size();i++) { for (int v=0;v<m_vehicles[i]->getNumWheels();v++) { @@ -188,7 +188,7 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() } -int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep) +int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) { int numSimulationSubSteps = 0; @@ -243,7 +243,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo return numSimulationSubSteps; } -void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) +void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { startProfiling(timeStep); @@ -291,7 +291,7 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -318,19 +318,19 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) if (body->getCollisionShape()) { bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter; - short collisionFilterMask = isDynamic? btBroadphaseProxy::AllFilter : btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); addCollisionObject(body,collisionFilterGroup,collisionFilterMask); } } -void btDiscreteDynamicsWorld::updateVehicles(float timeStep) +void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) { BEGIN_PROFILE("updateVehicles"); - for (unsigned int i=0;i<m_vehicles.size();i++) + for ( int i=0;i<m_vehicles.size();i++) { btRaycastVehicle* vehicle = m_vehicles[i]; vehicle->updateVehicle( timeStep); @@ -338,11 +338,11 @@ void btDiscreteDynamicsWorld::updateVehicles(float timeStep) END_PROFILE("updateVehicles"); } -void btDiscreteDynamicsWorld::updateActivationState(float timeStep) +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BEGIN_PROFILE("updateActivationState"); - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -377,11 +377,7 @@ void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint) void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) { - std::vector<btTypedConstraint*>::iterator cit = std::find(m_constraints.begin(),m_constraints.end(),constraint); - if (!(cit==m_constraints.end())) - { - m_constraints.erase(cit); - } + m_constraints.remove(constraint); } void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) @@ -391,11 +387,7 @@ void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) { - std::vector<btRaycastVehicle*>::iterator vit = std::find(m_vehicles.begin(),m_vehicles.end(),vehicle); - if (!(vit==m_vehicles.end())) - { - m_vehicles.erase(vit); - } + m_vehicles.remove(vehicle); } inline int btGetConstraintIslandId(const btTypedConstraint* lhs) @@ -409,19 +401,26 @@ inline int btGetConstraintIslandId(const btTypedConstraint* lhs) } -static bool btSortConstraintOnIslandPredicate(const btTypedConstraint* lhs, const btTypedConstraint* rhs) + +class btSortConstraintOnIslandPredicate { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; -} + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - BEGIN_PROFILE("solveConstraints"); - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { @@ -430,7 +429,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** m_sortedConstraints; int m_numConstraints; btIDebugDraw* m_debugDrawer; - + btStackAlloc* m_stackAlloc; InplaceSolverIslandCallback( @@ -438,17 +437,25 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btConstraintSolver* solver, btTypedConstraint** sortedConstraints, int numConstraints, - btIDebugDraw* debugDrawer) + btIDebugDraw* debugDrawer, + btStackAlloc* stackAlloc) :m_solverInfo(solverInfo), m_solver(solver), m_sortedConstraints(sortedConstraints), m_numConstraints(numConstraints), - m_debugDrawer(debugDrawer) + m_debugDrawer(debugDrawer), + m_stackAlloc(stackAlloc) { } - virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds, int islandId) + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) { //also add all non-contact constraints/joints for this island btTypedConstraint** startConstraint = 0; @@ -473,36 +480,35 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } - m_solver->solveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer); + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); } }; - - - - //sorted version of all btTypedConstraint, based on islandId - std::vector<btTypedConstraint*> sortedConstraints; + btAlignedObjectArray<btTypedConstraint*> sortedConstraints; sortedConstraints.resize( m_constraints.size()); int i; for (i=0;i<getNumConstraints();i++) { sortedConstraints[i] = m_constraints[i]; } + +// assert(0); + - std::sort(sortedConstraints.begin(),sortedConstraints.end(),btSortConstraintOnIslandPredicate); + + sortedConstraints.heapSort(btSortConstraintOnIslandPredicate()); btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0; - InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer); + InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc); /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); - END_PROFILE("solveConstraints"); } @@ -545,37 +551,6 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() } -static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color) -{ - - btVector3 halfExtents = (to-from)* 0.5f; - btVector3 center = (to+from) *0.5f; - int i,j; - - btVector3 edgecoord(1.f,1.f,1.f),pa,pb; - for (i=0;i<4;i++) - { - for (j=0;j<3;j++) - { - pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pa+=center; - - int othercoord = j%3; - edgecoord[othercoord]*=-1.f; - pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pb+=center; - - debugDrawer->drawLine(pa,pb,color); - } - edgecoord = btVector3(-1.f,-1.f,-1.f); - if (i<3) - edgecoord[i]*=-1.f; - } - - -} void btDiscreteDynamicsWorld::updateAabbs() { @@ -583,7 +558,7 @@ void btDiscreteDynamicsWorld::updateAabbs() btVector3 colorvec(1,0,0); btTransform predictedTrans; - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -594,34 +569,33 @@ void btDiscreteDynamicsWorld::updateAabbs() { btPoint3 minAabb,maxAabb; colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache; + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f)) + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) { bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb); } else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) - assert(0); body->setActivationState(DISABLE_SIMULATION); static bool reportMe = true; - if (reportMe) + if (reportMe && m_debugDrawer) { reportMe = false; - printf("Overflow in AABB, object removed from simulation \n"); - printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); - printf("Please include above information, your Platform, version of OS.\n"); - printf("Thanks.\n"); + m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); + m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); + m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + m_debugDrawer->reportErrorWarning("Thanks.\n"); } } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec); + m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); } } } @@ -630,11 +604,11 @@ void btDiscreteDynamicsWorld::updateAabbs() END_PROFILE("updateAabbs"); } -void btDiscreteDynamicsWorld::integrateTransforms(float timeStep) +void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BEGIN_PROFILE("integrateTransforms"); btTransform predictedTrans; - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -652,10 +626,10 @@ void btDiscreteDynamicsWorld::integrateTransforms(float timeStep) -void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep) +void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BEGIN_PROFILE("predictUnconstraintMotion"); - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -676,8 +650,9 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep) } -void btDiscreteDynamicsWorld::startProfiling(float timeStep) +void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) { + (void)timeStep; #ifdef USE_QUICKPROF @@ -724,10 +699,10 @@ class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIn public: - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) - : m_debugDrawer(debugDrawer), - m_worldTrans(worldTrans), - m_color(color) + DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : + m_debugDrawer(debugDrawer), + m_color(color), + m_worldTrans(worldTrans) { } @@ -738,6 +713,9 @@ public: virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) { + (void)partId; + (void)triangleIndex; + btVector3 wv0,wv1,wv2; wv0 = m_worldTrans*triangle[0]; wv1 = m_worldTrans*triangle[1]; @@ -771,7 +749,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, case SPHERE_SHAPE_PROXYTYPE: { const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); - float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin btVector3 start = worldTransform.getOrigin(); getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color); getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color); @@ -783,13 +761,13 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, case CONE_SHAPE_PROXYTYPE: { const btConeShape* coneShape = static_cast<const btConeShape*>(shape); - float radius = coneShape->getRadius();//+coneShape->getMargin(); - float height = coneShape->getHeight();//+coneShape->getMargin(); + btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); + btScalar height = coneShape->getHeight();//+coneShape->getMargin(); btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(radius,0.f,-0.5f*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(-radius,0.f,-0.5f*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,radius,-0.5f*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,-radius,-0.5f*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color); break; } @@ -797,8 +775,8 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); int upAxis = cylinder->getUpAxis(); - float radius = cylinder->getRadius(); - float halfHeight = cylinder->getHalfExtents()[upAxis]; + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtents()[upAxis]; btVector3 start = worldTransform.getOrigin(); btVector3 offsetHeight(0,0,0); offsetHeight[upAxis] = halfHeight; @@ -814,12 +792,12 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape; - //btVector3 aabbMax(1e30f,1e30f,1e30f); - //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); + //btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + //btVector3 aabbMax(100,100,100);//btScalar(1e30),btScalar(1e30),btScalar(1e30)); //todo pass camera, for some culling - btVector3 aabbMax(1e30f,1e30f,1e30f); - btVector3 aabbMin(-1e30f,-1e30f,-1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); @@ -830,8 +808,8 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; //todo: pass camera for some culling - btVector3 aabbMax(1e30f,1e30f,1e30f); - btVector3 aabbMin(-1e30f,-1e30f,-1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); //DebugDrawcallback drawCallback; DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 8575f8506f1..7ffa2c07ee6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -23,12 +23,12 @@ class btOverlappingPairCache; class btConstraintSolver; class btSimulationIslandManager; class btTypedConstraint; -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "../ConstraintSolver/btContactSolverInfo.h" class btRaycastVehicle; class btIDebugDraw; +#include "../../LinearMath/btAlignedObjectArray.h" -#include <vector> ///btDiscreteDynamicsWorld provides discrete rigid body simulation ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController @@ -40,14 +40,14 @@ protected: btSimulationIslandManager* m_islandManager; - std::vector<btTypedConstraint*> m_constraints; + btAlignedObjectArray<btTypedConstraint*> m_constraints; btIDebugDraw* m_debugDrawer; btVector3 m_gravity; //for variable timesteps - float m_localTime; + btScalar m_localTime; //for variable timesteps bool m_ownsIslandManager; @@ -56,29 +56,29 @@ protected: btContactSolverInfo m_solverInfo; - std::vector<btRaycastVehicle*> m_vehicles; + btAlignedObjectArray<btRaycastVehicle*> m_vehicles; int m_profileTimings; - void predictUnconstraintMotion(float timeStep); + void predictUnconstraintMotion(btScalar timeStep); - void integrateTransforms(float timeStep); + void integrateTransforms(btScalar timeStep); void calculateSimulationIslands(); void solveConstraints(btContactSolverInfo& solverInfo); - void updateActivationState(float timeStep); + void updateActivationState(btScalar timeStep); - void updateVehicles(float timeStep); + void updateVehicles(btScalar timeStep); - void startProfiling(float timeStep); + void startProfiling(btScalar timeStep); - virtual void internalSingleStepSimulation( float timeStep); + virtual void internalSingleStepSimulation( btScalar timeStep); void synchronizeMotionStates(); - void saveKinematicState(float timeStep); + void saveKinematicState(btScalar timeStep); public: @@ -90,7 +90,7 @@ public: virtual ~btDiscreteDynamicsWorld(); ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f); + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); virtual void updateAabbs(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 617ed98d2af..d3ea7db8e5c 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BT_DYNAMICS_WORLD_H #define BT_DYNAMICS_WORLD_H -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "../../BulletCollision/CollisionDispatch/btCollisionWorld.h" class btTypedConstraint; class btRaycastVehicle; class btConstraintSolver; @@ -39,17 +39,17 @@ class btDynamicsWorld : public btCollisionWorld ///stepSimulation proceeds the simulation over timeStep units ///if maxSubSteps > 0, it will interpolate time steps - virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f)=0; + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; virtual void updateAabbs() = 0; - virtual void addConstraint(btTypedConstraint* constraint) {}; + virtual void addConstraint(btTypedConstraint* constraint) { (void)constraint;}; - virtual void removeConstraint(btTypedConstraint* constraint) {}; + virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}; - virtual void addVehicle(btRaycastVehicle* vehicle) {}; + virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; - virtual void removeVehicle(btRaycastVehicle* vehicle) {}; + virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; virtual void setDebugDrawer(btIDebugDraw* debugDrawer) = 0; @@ -68,9 +68,9 @@ class btDynamicsWorld : public btCollisionWorld virtual int getNumConstraints() const { return 0; } - virtual btTypedConstraint* getConstraint(int index) { return 0; } + virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - virtual const btTypedConstraint* getConstraint(int index) const { return 0; } + virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 705c023d3a0..dbf73b85bcf 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -19,25 +19,25 @@ subject to the following restrictions: #include <LinearMath/btTransformUtil.h> #include <LinearMath/btMotionState.h> -float gLinearAirDamping = 1.f; +btScalar gLinearAirDamping = btScalar(1.); //'temporarily' global variables -float gDeactivationTime = 2.f; +btScalar gDeactivationTime = btScalar(2.); bool gDisableDeactivation = false; -float gLinearSleepingThreshold = 0.8f; -float gAngularSleepingThreshold = 1.0f; +btScalar gLinearSleepingThreshold = btScalar(0.8); +btScalar gAngularSleepingThreshold = btScalar(1.0); static int uniqueId = 0; -btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) +btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) : - m_gravity(0.0f, 0.0f, 0.0f), - m_totalForce(0.0f, 0.0f, 0.0f), - m_totalTorque(0.0f, 0.0f, 0.0f), - m_linearVelocity(0.0f, 0.0f, 0.0f), - m_angularVelocity(0.f,0.f,0.f), - m_angularFactor(1.f), - m_linearDamping(0.f), - m_angularDamping(0.5f), + m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), + m_angularFactor(btScalar(1.)), + m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_linearDamping(btScalar(0.)), + m_angularDamping(btScalar(0.5)), m_optionalMotionState(motionState), m_contactSolverType(0), m_frictionSolverType(0) @@ -72,15 +72,15 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap } #ifdef OBSOLETE_MOTIONSTATE_LESS -btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) +btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) : - m_gravity(0.0f, 0.0f, 0.0f), - m_totalForce(0.0f, 0.0f, 0.0f), - m_totalTorque(0.0f, 0.0f, 0.0f), - m_linearVelocity(0.0f, 0.0f, 0.0f), - m_angularVelocity(0.f,0.f,0.f), - m_linearDamping(0.f), - m_angularDamping(0.5f), + m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), + m_linearDamping(btScalar(0.)), + m_angularDamping(btScalar(0.5)), m_optionalMotionState(0), m_contactSolverType(0), m_frictionSolverType(0) @@ -110,16 +110,18 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi #endif //OBSOLETE_MOTIONSTATE_LESS -//#define EXPERIMENTAL_JITTER_REMOVAL 1 + + +#define EXPERIMENTAL_JITTER_REMOVAL 1 #ifdef EXPERIMENTAL_JITTER_REMOVAL //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate //doesn't work very well yet (value 0 disabled this damping) //note there this influences deactivation thresholds! -float gClippedAngvelThresholdSqr = 0.01f; -float gClippedLinearThresholdSqr = 0.01f; +btScalar gClippedAngvelThresholdSqr = btScalar(0.01); +btScalar gClippedLinearThresholdSqr = btScalar(0.01); #endif //EXPERIMENTAL_JITTER_REMOVAL -float gJitterVelocityDampingFactor = 1.f; +btScalar gJitterVelocityDampingFactor = btScalar(0.7); void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) { @@ -144,7 +146,7 @@ void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& pred void btRigidBody::saveKinematicState(btScalar timeStep) { //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities - if (timeStep != 0.f) + if (timeStep != btScalar(0.)) { //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform if (getMotionState()) @@ -169,9 +171,9 @@ void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const void btRigidBody::setGravity(const btVector3& acceleration) { - if (m_inverseMass != 0.0f) + if (m_inverseMass != btScalar(0.0)) { - m_gravity = acceleration * (1.0f / m_inverseMass); + m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); } } @@ -182,8 +184,8 @@ void btRigidBody::setGravity(const btVector3& acceleration) void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) { - m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f); - m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f); + m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } @@ -198,36 +200,36 @@ void btRigidBody::applyForces(btScalar step) applyCentralForce(m_gravity); - m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f); - m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f); + m_linearVelocity *= GEN_clamped((btScalar(1.) - step * gLinearAirDamping * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_angularVelocity *= GEN_clamped((btScalar(1.) - step * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); #define FORCE_VELOCITY_DAMPING 1 #ifdef FORCE_VELOCITY_DAMPING - float speed = m_linearVelocity.length(); + btScalar speed = m_linearVelocity.length(); if (speed < m_linearDamping) { - float dampVel = 0.005f; + btScalar dampVel = btScalar(0.005); if (speed > dampVel) { btVector3 dir = m_linearVelocity.normalized(); m_linearVelocity -= dir * dampVel; } else { - m_linearVelocity.setValue(0.f,0.f,0.f); + m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } } - float angSpeed = m_angularVelocity.length(); + btScalar angSpeed = m_angularVelocity.length(); if (angSpeed < m_angularDamping) { - float angDampVel = 0.005f; + btScalar angDampVel = btScalar(0.005); if (angSpeed > angDampVel) { btVector3 dir = m_angularVelocity.normalized(); m_angularVelocity -= dir * angDampVel; } else { - m_angularVelocity.setValue(0.f,0.f,0.f); + m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } } #endif //FORCE_VELOCITY_DAMPING @@ -242,19 +244,19 @@ void btRigidBody::proceedToTransform(const btTransform& newTrans) void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { - if (mass == 0.f) + if (mass == btScalar(0.)) { m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; - m_inverseMass = 0.f; + m_inverseMass = btScalar(0.); } else { m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - m_inverseMass = 1.0f / mass; + m_inverseMass = btScalar(1.0) / mass; } - m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f, - inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f, - inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f); + m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); } @@ -276,7 +278,7 @@ void btRigidBody::integrateVelocities(btScalar step) #define MAX_ANGVEL SIMD_HALF_PI /// clamp angular velocity. collision calculations will fail on higher angular velocities - float angvel = m_angularVelocity.length(); + btScalar angvel = m_angularVelocity.length(); if (angvel*step > MAX_ANGVEL) { m_angularVelocity *= (MAX_ANGVEL/step) /angvel; @@ -295,7 +297,7 @@ btQuaternion btRigidBody::getOrientation() const void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { - m_interpolationWorldTransform = m_worldTransform; + m_interpolationWorldTransform = xform;//m_worldTransform; m_interpolationLinearVelocity = getLinearVelocity(); m_interpolationAngularVelocity = getAngularVelocity(); m_worldTransform = xform; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 43869363cdf..9da545adb68 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -16,26 +16,22 @@ subject to the following restrictions: #ifndef RIGIDBODY_H #define RIGIDBODY_H -#include <vector> -#include <LinearMath/btPoint3.h> -#include <LinearMath/btTransform.h> -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" - - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btTransform.h" +#include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../../BulletCollision/CollisionDispatch/btCollisionObject.h" class btCollisionShape; class btMotionState; -extern float gLinearAirDamping; -extern bool gUseEpa; +extern btScalar gLinearAirDamping; -extern float gDeactivationTime; +extern btScalar gDeactivationTime; extern bool gDisableDeactivation; -extern float gLinearSleepingThreshold; -extern float gAngularSleepingThreshold; +extern btScalar gLinearSleepingThreshold; +extern btScalar gAngularSleepingThreshold; /// btRigidBody class for btRigidBody Dynamics @@ -65,10 +61,10 @@ public: #ifdef OBSOLETE_MOTIONSTATE_LESS //not supported, please use btMotionState - btRigidBody(float mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f); + btRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); #endif //OBSOLETE_MOTIONSTATE_LESS - btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f); + btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); void proceedToTransform(const btTransform& newTrans); @@ -157,7 +153,7 @@ public: void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) { - if (m_inverseMass != 0.f) + if (m_inverseMass != btScalar(0.)) { applyCentralImpulse(impulse); if (m_angularFactor) @@ -168,9 +164,9 @@ public: } //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,float impulseMagnitude) + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) { - if (m_inverseMass != 0.f) + if (m_inverseMass != btScalar(0.)) { m_linearVelocity += linearComponent*impulseMagnitude; if (m_angularFactor) @@ -182,8 +178,8 @@ public: void clearForces() { - m_totalForce.setValue(0.0f, 0.0f, 0.0f); - m_totalTorque.setValue(0.0f, 0.0f, 0.0f); + m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } void updateInertiaTensor(); @@ -238,7 +234,7 @@ public: - inline float computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const + inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); @@ -250,13 +246,13 @@ public: } - inline float computeAngularImpulseDenominator(const btVector3& axis) const + inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const { btVector3 vec = axis * getInvInertiaTensorWorld(); return axis.dot(vec); } - inline void updateDeactivation(float timeStep) + inline void updateDeactivation(btScalar timeStep) { if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) return; @@ -267,7 +263,7 @@ public: m_deactivationTime += timeStep; } else { - m_deactivationTime=0.f; + m_deactivationTime=btScalar(0.); setActivationState(0); } @@ -280,7 +276,7 @@ public: return false; //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == 0.f)) + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) return false; if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) @@ -328,11 +324,11 @@ public: int m_contactSolverType; int m_frictionSolverType; - void setAngularFactor(float angFac) + void setAngularFactor(btScalar angFac) { m_angularFactor = angFac; } - float getAngularFactor() const + btScalar getAngularFactor() const { return m_angularFactor; } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index fe0124c041a..4ebcb8e7517 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -22,6 +22,14 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" void btBulletDynamicsProbe () {} + + btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) @@ -41,8 +49,12 @@ btSimpleDynamicsWorld::~btSimpleDynamicsWorld() delete m_constraintSolver; } -int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep) +int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) { + (void)fixedTimeStep; + (void)maxSubSteps; + + ///apply gravity, predict motion predictUnconstraintMotion(timeStep); @@ -63,7 +75,7 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer); + m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc); } ///integrate transforms @@ -81,7 +93,7 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -110,7 +122,7 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) void btSimpleDynamicsWorld::updateAabbs() { btTransform predictedTrans; - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -127,10 +139,10 @@ void btSimpleDynamicsWorld::updateAabbs() } } -void btSimpleDynamicsWorld::integrateTransforms(float timeStep) +void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) { btTransform predictedTrans; - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -147,9 +159,9 @@ void btSimpleDynamicsWorld::integrateTransforms(float timeStep) -void btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep) +void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -172,7 +184,7 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep) void btSimpleDynamicsWorld::synchronizeMotionStates() { //todo: iterate over awake simulation islands! - for (unsigned int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index cdc0c5559f6..25f4ccd8e68 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -37,9 +37,9 @@ protected: btIDebugDraw* m_debugDrawer; - void predictUnconstraintMotion(float timeStep); + void predictUnconstraintMotion(btScalar timeStep); - void integrateTransforms(float timeStep); + void integrateTransforms(btScalar timeStep); btVector3 m_gravity; @@ -53,7 +53,7 @@ public: virtual ~btSimpleDynamicsWorld(); ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead - virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f); + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); virtual void setDebugDrawer(btIDebugDraw* debugDrawer) { diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index c85fead5334..d53de7f3687 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -18,6 +18,7 @@ #include "BulletDynamics/Dynamics/btDynamicsWorld.h" #include "btVehicleRaycaster.h" #include "btWheelInfo.h" +#include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" @@ -28,7 +29,7 @@ static btRigidBody s_fixedObject( 0,0,0); btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) :m_vehicleRaycaster(raycaster), -m_pitchControl(0.f) +m_pitchControl(btScalar(0.)) { m_chassisBody = chassis; m_indexRightAxis = 0; @@ -40,8 +41,9 @@ m_pitchControl(0.f) void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) { - m_currentVehicleSpeedKmHour = 0.f; - m_steeringValue = 0.f; + (void)tuning; + m_currentVehicleSpeedKmHour = btScalar(0.); + m_steeringValue = btScalar(0.); } @@ -105,12 +107,12 @@ void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT // up.normalize(); //rotate around steering over de wheelAxleWS - float steering = wheel.m_steering; + btScalar steering = wheel.m_steering; btQuaternion steeringOrn(up,steering);//wheel.m_steering); btMatrix3x3 steeringMat(steeringOrn); - btQuaternion rotatingOrn(right,wheel.m_rotation); + btQuaternion rotatingOrn(right,-wheel.m_rotation); btMatrix3x3 rotatingMat(rotatingOrn); btMatrix3x3 basis2( @@ -133,11 +135,11 @@ void btRaycastVehicle::resetSuspension() { btWheelInfo& wheel = m_wheelInfo[i]; wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); - wheel.m_suspensionRelativeVelocity = 0.0f; + wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; - //wheel_info.setContactFriction(0.0f); - wheel.m_clippedInvContactDotSuspension = 1.0f; + //wheel_info.setContactFriction(btScalar(0.0)); + wheel.m_clippedInvContactDotSuspension = btScalar(1.0); } } @@ -170,7 +172,7 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) wheel.m_raycastInfo.m_contactPointWS = source + rayvector; const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; - btScalar param = 0.f; + btScalar param = btScalar(0.); btVehicleRaycaster::btVehicleRaycasterResult rayResults; @@ -195,8 +197,8 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; //clamp on max suspension travel - float minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*0.01f; - float maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*0.01f; + btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); + btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; @@ -217,14 +219,14 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - if ( denominator >= -0.1f) + if ( denominator >= btScalar(-0.1)) { - wheel.m_suspensionRelativeVelocity = 0.0f; - wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f; + wheel.m_suspensionRelativeVelocity = btScalar(0.0); + wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { - btScalar inv = -1.f / denominator; + btScalar inv = btScalar(-1.) / denominator; wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_clippedInvContactDotSuspension = inv; } @@ -233,9 +235,9 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) { //put wheel info as in rest position wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); - wheel.m_suspensionRelativeVelocity = 0.0f; + wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; - wheel.m_clippedInvContactDotSuspension = 1.0f; + wheel.m_clippedInvContactDotSuspension = btScalar(1.0); } return depth; @@ -267,7 +269,7 @@ void btRaycastVehicle::updateVehicle( btScalar step ) } - m_currentVehicleSpeedKmHour = 3.6f * getRigidBody()->getLinearVelocity().length(); + m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length(); const btTransform& chassisTrans = getChassisWorldTransform(); @@ -276,9 +278,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) chassisTrans.getBasis()[1][m_indexForwardAxis], chassisTrans.getBasis()[2][m_indexForwardAxis]); - if (forwardW.dot(getRigidBody()->getLinearVelocity()) < 0.f) + if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) { - m_currentVehicleSpeedKmHour *= -1.f; + m_currentVehicleSpeedKmHour *= btScalar(-1.); } // @@ -300,9 +302,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) //apply suspension force btWheelInfo& wheel = m_wheelInfo[i]; - float suspensionForce = wheel.m_wheelsSuspensionForce; + btScalar suspensionForce = wheel.m_wheelsSuspensionForce; - float gMaxSuspensionForce = 6000.f; + btScalar gMaxSuspensionForce = btScalar(6000.); if (suspensionForce > gMaxSuspensionForce) { suspensionForce = gMaxSuspensionForce; @@ -347,7 +349,7 @@ void btRaycastVehicle::updateVehicle( btScalar step ) wheel.m_rotation += wheel.m_deltaRotation; } - wheel.m_deltaRotation *= 0.99f;//damping of rotation when not in contact + wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact } @@ -394,17 +396,18 @@ btWheelInfo& btRaycastVehicle::getWheelInfo(int index) return m_wheelInfo[index]; } -void btRaycastVehicle::setBrake(float brake,int wheelIndex) +void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) { btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); - getWheelInfo(wheelIndex).m_brake; + getWheelInfo(wheelIndex).m_brake = brake; } void btRaycastVehicle::updateSuspension(btScalar deltaTime) { + (void)deltaTime; - btScalar chassisMass = 1.f / m_chassisBody->getInvMass(); + btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); for (int w_it=0; w_it<getNumWheels(); w_it++) { @@ -429,7 +432,7 @@ void btRaycastVehicle::updateSuspension(btScalar deltaTime) btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; { btScalar susp_damping; - if ( projected_rel_vel < 0.0f ) + if ( projected_rel_vel < btScalar(0.0) ) { susp_damping = wheel_info.m_wheelsDampingCompression; } @@ -443,20 +446,77 @@ void btRaycastVehicle::updateSuspension(btScalar deltaTime) // RESULT wheel_info.m_wheelsSuspensionForce = force * chassisMass; - if (wheel_info.m_wheelsSuspensionForce < 0.f) + if (wheel_info.m_wheelsSuspensionForce < btScalar(0.)) { - wheel_info.m_wheelsSuspensionForce = 0.f; + wheel_info.m_wheelsSuspensionForce = btScalar(0.); } } else { - wheel_info.m_wheelsSuspensionForce = 0.0f; + wheel_info.m_wheelsSuspensionForce = btScalar(0.0); } } } -float sideFrictionStiffness2 = 1.0f; + +struct btWheelContactPoint +{ + btRigidBody* m_body0; + btRigidBody* m_body1; + btVector3 m_frictionPositionWorld; + btVector3 m_frictionDirectionWorld; + btScalar m_jacDiagABInv; + btScalar m_maxImpulse; + + + btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse) + :m_body0(body0), + m_body1(body1), + m_frictionPositionWorld(frictionPosWorld), + m_frictionDirectionWorld(frictionDirectionWorld), + m_maxImpulse(maxImpulse) + { + btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); + btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); + btScalar relaxation = 1.f; + m_jacDiagABInv = relaxation/(denom0+denom1); + } + + + +}; + +btScalar calcRollingFriction(btWheelContactPoint& contactPoint) +{ + + btScalar j1=0.f; + + const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; + + btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); + btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); + + btScalar maxImpulse = contactPoint.m_maxImpulse; + + btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * contactPoint.m_jacDiagABInv; + GEN_set_min(j1, maxImpulse); + GEN_set_max(j1, -maxImpulse); + + return j1; +} + + + + +btScalar sideFrictionStiffness2 = btScalar(1.0); void btRaycastVehicle::updateFriction(btScalar timeStep) { @@ -481,8 +541,8 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; if (groundObject) numWheelsOnGround++; - sideImpulse[i] = 0.f; - forwardImpulse[i] = 0.f; + sideImpulse[i] = btScalar(0.); + forwardImpulse[i] = btScalar(0.); } @@ -517,7 +577,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, - 0.f, axle[i],sideImpulse[i],timeStep); + btScalar(0.), axle[i],sideImpulse[i],timeStep); sideImpulse[i] *= sideFrictionStiffness2; @@ -527,7 +587,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) } } - btScalar sideFactor = 1.f; + btScalar sideFactor = btScalar(1.); btScalar fwdFactor = 0.5; bool sliding = false; @@ -537,25 +597,46 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btWheelInfo& wheelInfo = m_wheelInfo[wheel]; class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; + btScalar rollingFriction = 0.f; - forwardImpulse[wheel] = 0.f; - m_wheelInfo[wheel].m_skidInfo= 1.f; + if (groundObject) + { + if (wheelInfo.m_engineForce != 0.f) + { + rollingFriction = wheelInfo.m_engineForce* timeStep; + } else + { + btScalar defaultRollingFrictionImpulse = 0.f; + btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; + btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,forwardWS[wheel],maxImpulse); + rollingFriction = calcRollingFriction(contactPt); + } + } + + //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) + + + + + forwardImpulse[wheel] = btScalar(0.); + m_wheelInfo[wheel].m_skidInfo= btScalar(1.); if (groundObject) { - m_wheelInfo[wheel].m_skidInfo= 1.f; + m_wheelInfo[wheel].m_skidInfo= btScalar(1.); btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; btScalar maximpSide = maximp; btScalar maximpSquared = maximp * maximpSide; + - forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep; + forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; - float x = (forwardImpulse[wheel] ) * fwdFactor; - float y = (sideImpulse[wheel] ) * sideFactor; + btScalar x = (forwardImpulse[wheel] ) * fwdFactor; + btScalar y = (sideImpulse[wheel] ) * sideFactor; - float impulseSquared = (x*x + y*y); + btScalar impulseSquared = (x*x + y*y); if (impulseSquared > maximpSquared) { @@ -577,9 +658,9 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) { for (int wheel = 0;wheel < getNumWheels(); wheel++) { - if (sideImpulse[wheel] != 0.f) + if (sideImpulse[wheel] != btScalar(0.)) { - if (m_wheelInfo[wheel].m_skidInfo< 1.f) + if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) { forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; @@ -597,11 +678,11 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - m_chassisBody->getCenterOfMassPosition(); - if (forwardImpulse[wheel] != 0.f) + if (forwardImpulse[wheel] != btScalar(0.)) { m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos); } - if (sideImpulse[wheel] != 0.f) + if (sideImpulse[wheel] != btScalar(0.)) { class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index fa961e468d7..f4249599615 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -11,11 +11,11 @@ #ifndef RAYCASTVEHICLE_H #define RAYCASTVEHICLE_H -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "../Dynamics/btRigidBody.h" +#include "../ConstraintSolver/btTypedConstraint.h" #include "btVehicleRaycaster.h" class btDynamicsWorld; -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btAlignedObjectArray.h" #include "btWheelInfo.h" class btVehicleTuning; @@ -29,18 +29,18 @@ public: public: btVehicleTuning() - :m_suspensionStiffness(5.88f), - m_suspensionCompression(0.83f), - m_suspensionDamping(0.88f), - m_maxSuspensionTravelCm(500.f), - m_frictionSlip(10.5f) + :m_suspensionStiffness(btScalar(5.88)), + m_suspensionCompression(btScalar(0.83)), + m_suspensionDamping(btScalar(0.88)), + m_maxSuspensionTravelCm(btScalar(500.)), + m_frictionSlip(btScalar(10.5)) { } - float m_suspensionStiffness; - float m_suspensionCompression; - float m_suspensionDamping; - float m_maxSuspensionTravelCm; - float m_frictionSlip; + btScalar m_suspensionStiffness; + btScalar m_suspensionCompression; + btScalar m_suspensionDamping; + btScalar m_maxSuspensionTravelCm; + btScalar m_frictionSlip; }; private: @@ -48,9 +48,9 @@ private: btScalar m_tau; btScalar m_damping; btVehicleRaycaster* m_vehicleRaycaster; - float m_pitchControl; - float m_steeringValue; - float m_currentVehicleSpeedKmHour; + btScalar m_pitchControl; + btScalar m_steeringValue; + btScalar m_currentVehicleSpeedKmHour; btRigidBody* m_chassisBody; @@ -105,9 +105,9 @@ public: void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); - void setBrake(float brake,int wheelIndex); + void setBrake(btScalar brake,int wheelIndex); - void setPitchControl(float pitch) + void setPitchControl(btScalar pitch) { m_pitchControl = pitch; } @@ -142,6 +142,26 @@ public: return m_indexForwardAxis; } + + ///Worldspace forward vector + btVector3 getForwardVector() const + { + const btTransform& chassisTrans = getChassisWorldTransform(); + + btVector3 forwardW ( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); + + return forwardW; + } + + ///Velocity of vehicle (positive if velocity vector has same direction as foward vector) + btScalar getCurrentSpeedKmHour() const + { + return m_currentVehicleSpeedKmHour; + } + virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) { m_indexRightAxis = rightIndex; @@ -156,6 +176,7 @@ public: virtual void solveConstraint(btScalar timeStep) { + (void)timeStep; //not yet } diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h index 5be3693fe73..64a47fcaada 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h @@ -11,7 +11,7 @@ #ifndef VEHICLE_RAYCASTER_H #define VEHICLE_RAYCASTER_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" /// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting struct btVehicleRaycaster @@ -21,7 +21,7 @@ virtual ~btVehicleRaycaster() } struct btVehicleRaycasterResult { - btVehicleRaycasterResult() :m_distFraction(-1.f){}; + btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){}; btVector3 m_hitPointInWorld; btVector3 m_hitNormalInWorld; btScalar m_distFraction; diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp index 43baf56edbc..ef93c16fffc 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -21,6 +21,7 @@ btScalar btWheelInfo::getSuspensionRestLength() const void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) { + (void)raycastInfo; if (m_raycastInfo.m_isInContact) @@ -31,14 +32,14 @@ void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - if ( project >= -0.1f) + if ( project >= btScalar(-0.1)) { - m_suspensionRelativeVelocity = 0.0f; - m_clippedInvContactDotSuspension = 1.0f / 0.1f; + m_suspensionRelativeVelocity = btScalar(0.0); + m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { - btScalar inv = -1.f / project; + btScalar inv = btScalar(-1.) / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } @@ -48,8 +49,8 @@ void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); - m_suspensionRelativeVelocity = 0.0f; + m_suspensionRelativeVelocity = btScalar(0.0); m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; - m_clippedInvContactDotSuspension = 1.0f; + m_clippedInvContactDotSuspension = btScalar(1.0); } } diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h index 83a3b114ab9..2e349b3fde4 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -11,8 +11,8 @@ #ifndef WHEEL_INFO_H #define WHEEL_INFO_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" class btRigidBody; @@ -25,10 +25,10 @@ struct btWheelInfoConstructionInfo btScalar m_maxSuspensionTravelCm; btScalar m_wheelRadius; - float m_suspensionStiffness; - float m_wheelsDampingCompression; - float m_wheelsDampingRelaxation; - float m_frictionSlip; + btScalar m_suspensionStiffness; + btScalar m_wheelsDampingCompression; + btScalar m_wheelsDampingRelaxation; + btScalar m_frictionSlip; bool m_bIsFrontWheel; }; @@ -92,12 +92,12 @@ struct btWheelInfo m_wheelDirectionCS = ci.m_wheelDirectionCS; m_wheelAxleCS = ci.m_wheelAxleCS; m_frictionSlip = ci.m_frictionSlip; - m_steering = 0.f; - m_engineForce = 0.f; - m_rotation = 0.f; - m_deltaRotation = 0.f; - m_brake = 0.f; - m_rollInfluence = 0.1f; + m_steering = btScalar(0.); + m_engineForce = btScalar(0.); + m_rotation = btScalar(0.); + m_deltaRotation = btScalar(0.); + m_brake = btScalar(0.); + m_rollInfluence = btScalar(0.1); m_bIsFrontWheel = ci.m_bIsFrontWheel; } diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 2eacb8e242b..429163c8138 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -17,8 +17,8 @@ subject to the following restrictions: #ifndef AABB_UTIL2 #define AABB_UTIL2 -#include "LinearMath/btVector3.h" -#include "LinearMath/btSimdMinMax.h" +#include "btVector3.h" +#include "btSimdMinMax.h" #define btMin(a,b) ((a < b ? a : b)) @@ -73,8 +73,8 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, const btVector3& aabbMax, btScalar& param, btVector3& normal) { - btVector3 aabbHalfExtent = (aabbMax-aabbMin)* 0.5f; - btVector3 aabbCenter = (aabbMax+aabbMin)* 0.5f; + btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5); + btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5); btVector3 source = rayFrom - aabbCenter; btVector3 target = rayTo - aabbCenter; int sourceOutcode = btOutcode(source,aabbHalfExtent); @@ -110,7 +110,7 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, } bit<<=1; } - normSign = -1.f; + normSign = btScalar(-1.); } if (lambda_enter <= lambda_exit) { diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp index 19b811b0ce8..1f5877fa37e 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btAlignedAllocator.h" -#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) +#if defined (BT_HAS_ALIGNED_ALOCATOR) #include <malloc.h> void* btAlignedAlloc (int size, int alignment) @@ -31,6 +31,26 @@ void btAlignedFree (void* ptr) #else +#ifdef __CELLOS_LV2__ + +#include <stdlib.h> + +int numAllocs = 0; +int numFree = 0; + +void* btAlignedAlloc (int size, int alignment) +{ + numAllocs++; + return memalign(alignment, size); +} + +void btAlignedFree (void* ptr) +{ + numFree++; + free(ptr); +} + +#else ///todo ///will add some multi-platform version that works without _aligned_malloc/_aligned_free @@ -41,7 +61,10 @@ void* btAlignedAlloc (int size, int alignment) void btAlignedFree (void* ptr) { - delete ptr; + delete [] (char*) ptr; } +#endif // #endif + + diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.h b/extern/bullet2/src/LinearMath/btAlignedAllocator.h index 159399ce7d4..07585717f45 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.h +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.h @@ -36,11 +36,13 @@ class btAlignedAllocator { typedef btAlignedAllocator< T , Alignment > self_type; public: + //just going down a list: btAlignedAllocator() {} - + /* btAlignedAllocator( const self_type & ) {} - + */ + template < typename Other > btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {} @@ -53,6 +55,7 @@ public: pointer address ( reference ref ) const { return &ref; } const_pointer address ( const_reference ref ) const { return &ref; } pointer allocate ( size_type n , const_pointer * hint = 0 ) { + (void)hint; return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment )); } void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); } diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index 3bfdb36f4c7..8bef5eb5d06 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -20,18 +20,37 @@ subject to the following restrictions: #include "btScalar.h" // has definitions like SIMD_FORCE_INLINE #include "btAlignedAllocator.h" +///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW +///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors +///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator= +///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and +///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240 + +#define BT_USE_PLACEMENT_NEW 1 +//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise... + +#ifdef BT_USE_MEMCPY +#include <memory.h> +#include <string.h> +#endif //BT_USE_MEMCPY + +#ifdef BT_USE_PLACEMENT_NEW +#include <new> //for placement new +#endif //BT_USE_PLACEMENT_NEW + + ///btAlignedObjectArray uses a subset of the stl::vector interface for its methods ///It is developed to replace stl::vector to avoid STL alignment issues to add SIMD/SSE data template <typename T> //template <class T> class btAlignedObjectArray { + btAlignedAllocator<T , 16> m_allocator; + int m_size; int m_capacity; T* m_data; - btAlignedAllocator<T , 16> m_allocator; - protected: SIMD_FORCE_INLINE int allocSize(int size) { @@ -40,8 +59,12 @@ class btAlignedObjectArray SIMD_FORCE_INLINE void copy(int start,int end, T* dest) { int i; - for (i=0;i<m_size;++i) + for (i=start;i<end;++i) +#ifdef BT_USE_PLACEMENT_NEW + new (&dest[i]) T(m_data[i]); +#else dest[i] = m_data[i]; +#endif //BT_USE_PLACEMENT_NEW } SIMD_FORCE_INLINE void init() @@ -53,7 +76,7 @@ class btAlignedObjectArray SIMD_FORCE_INLINE void destroy(int first,int last) { int i; - for (i=0; i<m_size;i++) + for (i=first; i<last;i++) { m_data[i].~T(); } @@ -74,6 +97,8 @@ class btAlignedObjectArray } } + + public: @@ -123,17 +148,50 @@ class btAlignedObjectArray m_data[m_size].~T(); } - SIMD_FORCE_INLINE void resize(int newsize) + SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T()) { - if (newsize > size()) + int curSize = size(); + + if (newsize < size()) + { + for(int i = curSize; i < newsize; i++) + { + m_data[i].~T(); + } + } else { - reserve(newsize); + if (newsize > size()) + { + reserve(newsize); + } +#ifdef BT_USE_PLACEMENT_NEW + for (int i=curSize;i<newsize;i++) + { + new ( &m_data[i]) T(fillData); + } +#endif //BT_USE_PLACEMENT_NEW + } m_size = newsize; } + SIMD_FORCE_INLINE T& expand( const T& fillValue=T()) + { + int sz = size(); + if( sz == capacity() ) + { + reserve( allocSize(size()) ); + } + m_size++; +#ifdef BT_USE_PLACEMENT_NEW + new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory) +#endif + + return m_data[sz]; + } + SIMD_FORCE_INLINE void push_back(const T& _Val) { @@ -143,8 +201,12 @@ class btAlignedObjectArray reserve( allocSize(size()) ); } - m_data[size()] = _Val; - //::new ( m_data[m_size] ) T(_Val); +#ifdef BT_USE_PLACEMENT_NEW + new ( &m_data[m_size] ) T(_Val); +#else + m_data[size()] = _Val; +#endif //BT_USE_PLACEMENT_NEW + m_size++; } @@ -154,24 +216,152 @@ class btAlignedObjectArray { // determine new minimum length of allocated storage if (capacity() < _Count) { // not enough room, reallocate - if (capacity() < _Count) - { - T* s = (T*)allocate(_Count); + T* s = (T*)allocate(_Count); + + copy(0, size(), s); + + destroy(0,size()); - copy(0, size(), s); + deallocate(); + + m_data = s; + + m_capacity = _Count; + + } + } - destroy(0,size()); - deallocate(); + class less + { + public: - m_data = s; - - m_capacity = _Count; + bool operator() ( const T& a, const T& b ) + { + return ( a < b ); + } + }; + + ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ + template <typename L> + void downHeap(T *pArr, int k, int n,L CompareFunc) + { + /* PRE: a[k+1..N] is a heap */ + /* POST: a[k..N] is a heap */ + + T temp = pArr[k - 1]; + /* k has child(s) */ + while (k <= n/2) + { + int child = 2*k; + + if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child])) + { + child++; + } + /* pick larger child */ + if (CompareFunc(temp , pArr[child - 1])) + { + /* move child up */ + pArr[k - 1] = pArr[child - 1]; + k = child; } + else + { + break; + } + } + pArr[k - 1] = temp; + } /*downHeap*/ + + void swap(int index0,int index1) + { +#ifdef BT_USE_MEMCPY + char temp[sizeof(T)]; + memcpy(temp,&m_data[index0],sizeof(T)); + memcpy(&m_data[index0],&m_data[index1],sizeof(T)); + memcpy(&m_data[index1],temp,sizeof(T)); +#else + T temp = m_data[index0]; + m_data[index0] = m_data[index1]; + m_data[index1] = temp; +#endif //BT_USE_PLACEMENT_NEW + + } + + template <typename L> + void heapSort(L CompareFunc) + { + /* sort a[0..N-1], N.B. 0 to N-1 */ + int k; + int n = m_size; + for (k = n/2; k > 0; k--) + { + downHeap(m_data, k, n, CompareFunc); + } + + /* a[1..N] is now a heap */ + while ( n>=1 ) + { + swap(0,n-1); /* largest of a[0..n-1] */ + + + n = n - 1; + /* restore a[1..i-1] heap */ + downHeap(m_data, 1, n, CompareFunc); + } + } + + ///non-recursive binary search, assumes sorted array + int findBinarySearch(const T& key) const + { + int first = 0; + int last = size(); + + //assume sorted array + while (first <= last) { + int mid = (first + last) / 2; // compute mid point. + if (key > m_data[mid]) + first = mid + 1; // repeat search in top half. + else if (key < m_data[mid]) + last = mid - 1; // repeat search in bottom half. + else + return mid; // found it. return position ///// + } + return size(); // failed to find key + } + + + int findLinearSearch(const T& key) const + { + int index=size(); + int i; + + for (i=0;i<size();i++) + { + if (m_data[i] == key) + { + index = i; + break; } } + return index; + } + + void remove(const T& key) + { + + int findIndex = findLinearSearch(key); + if (findIndex<size()) + { + swap( findIndex,size()-1); + pop_back(); + } + } }; #endif //BT_OBJECT_ARRAY__ + + diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h index 6b85b37fb9e..d4ec8e8879c 100644 --- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -31,12 +31,7 @@ struct btDefaultMotionState : public btMotionState m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; } - ///Bullet gives a callback for objects that are about to be deactivated (put asleep) - /// You can intercept this callback for your own bookkeeping. - ///Also you can return false to disable deactivation for this object this frame. - virtual bool deactivationCallback(void* userPointer) { - return true; - } + }; diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp index 5036894b2b3..3d0fb122a6b 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp @@ -16,14 +16,23 @@ subject to the following restrictions: #include "btGeometryUtil.h" -bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, float margin) + +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" void btBulletMathProbe () {} + + +bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin) { int numbrushes = planeEquations.size(); for (int i=0;i<numbrushes;i++) { const btVector3& N1 = planeEquations[i]; - float dist = float(N1.dot(point))+float(N1[3])-margin; - if (dist>0.f) + btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin; + if (dist>btScalar(0.)) { return false; } @@ -33,14 +42,14 @@ bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& } -bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, float margin) +bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin) { int numvertices = vertices.size(); for (int i=0;i<numvertices;i++) { const btVector3& N1 = vertices[i]; - float dist = float(planeNormal.dot(N1))+float(planeNormal[3])-margin; - if (dist>0.f) + btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin; + if (dist>btScalar(0.)) { return false; } @@ -54,7 +63,7 @@ bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector for (int i=0;i<numbrushes;i++) { const btVector3& N1 = planeEquations[i]; - if (planeEquation.dot(N1) > 0.999f) + if (planeEquation.dot(N1) > btScalar(0.999)) { return false; } @@ -83,11 +92,11 @@ void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector btVector3 planeEquation,edge0,edge1; edge0 = N2-N1; edge1 = N3-N1; - float normalSign = 1.f; + btScalar normalSign = btScalar(1.); for (int ww=0;ww<2;ww++) { planeEquation = normalSign * edge0.cross(edge1); - if (planeEquation.length2() > 0.0001f) + if (planeEquation.length2() > btScalar(0.0001)) { planeEquation.normalize(); if (notExist(planeEquation,planeEquationsOut)) @@ -95,13 +104,13 @@ void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector planeEquation[3] = -planeEquation.dot(N1); //check if inside, and replace supportingVertexOut if needed - if (areVerticesBehindPlane(planeEquation,vertices,0.01f)) + if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01))) { planeEquationsOut.push_back(planeEquation); } } } - normalSign = -1.f; + normalSign = btScalar(-1.); } } @@ -132,9 +141,9 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt btVector3 n3n1; n3n1 = N3.cross(N1); btVector3 n1n2; n1n2 = N1.cross(N2); - if ( ( n2n3.length2() > 0.0001f ) && - ( n3n1.length2() > 0.0001f ) && - ( n1n2.length2() > 0.0001f ) ) + if ( ( n2n3.length2() > btScalar(0.0001) ) && + ( n3n1.length2() > btScalar(0.0001) ) && + ( n1n2.length2() > btScalar(0.0001) ) ) { //point P out of 3 plane equations: @@ -143,10 +152,10 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt // N1 . ( N2 * N3 ) - float quotient = (N1.dot(n2n3)); - if (btFabs(quotient) > 0.000001f) + btScalar quotient = (N1.dot(n2n3)); + if (btFabs(quotient) > btScalar(0.000001)) { - quotient = -1.f / quotient; + quotient = btScalar(-1.) / quotient; n2n3 *= N1[3]; n3n1 *= N2[3]; n1n2 *= N3[3]; @@ -156,7 +165,7 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt potentialVertex *= quotient; //check if inside, and replace supportingVertexOut if needed - if (isPointInsidePlanes(planeEquations,potentialVertex,0.01f)) + if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01))) { verticesOut.push_back(potentialVertex); } diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h index 018ffa72296..766cd75c383 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.h +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.h @@ -28,11 +28,11 @@ class btGeometryUtil static void getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut ); - static bool isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, float margin); + static bool isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar margin); - static bool isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, float margin); + static bool isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin); - static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, float margin); + static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin); }; diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index 86db735ce94..5f40ca39157 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -28,7 +28,7 @@ DEALINGS IN THE SOFTWARE. #ifndef IDEBUG_DRAW__H #define IDEBUG_DRAW__H -#include "LinearMath/btVector3.h" +#include "btVector3.h" class btIDebugDraw @@ -56,14 +56,45 @@ class btIDebugDraw virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; - virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,float distance,int lifeTime,const btVector3& color)=0; + virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0; + + virtual void reportErrorWarning(const char* warningString) = 0; virtual void setDebugMode(int debugMode) =0; virtual int getDebugMode() const = 0; + inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color) + { + btVector3 halfExtents = (to-from)* 0.5f; + btVector3 center = (to+from) *0.5f; + int i,j; + + btVector3 edgecoord(1.f,1.f,1.f),pa,pb; + for (i=0;i<4;i++) + { + for (j=0;j<3;j++) + { + pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pa+=center; + + int othercoord = j%3; + edgecoord[othercoord]*=-1.f; + pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pb+=center; + + drawLine(pa,pb,color); + } + edgecoord = btVector3(-1.f,-1.f,-1.f); + if (i<3) + edgecoord[i]*=-1.f; + } + } }; + #endif //IDEBUG_DRAW__H diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index c3cc90a82c7..fda348b42ab 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -16,10 +16,10 @@ subject to the following restrictions: #ifndef btMatrix3x3_H #define btMatrix3x3_H -#include "LinearMath/btScalar.h" +#include "btScalar.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btQuaternion.h" +#include "btVector3.h" +#include "btQuaternion.h" class btMatrix3x3 { @@ -45,12 +45,14 @@ class btMatrix3x3 { zx, zy, zz); } - btVector3 getColumn(int i) const + SIMD_FORCE_INLINE btVector3 getColumn(int i) const { return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); } + + - const btVector3& getRow(int i) const + SIMD_FORCE_INLINE const btVector3& getRow(int i) const { return m_el[i]; } @@ -58,13 +60,13 @@ class btMatrix3x3 { SIMD_FORCE_INLINE btVector3& operator[](int i) { - assert(0 <= i && i < 3); + btFullAssert(0 <= i && i < 3); return m_el[i]; } - const btVector3& operator[](int i) const + SIMD_FORCE_INLINE const btVector3& operator[](int i) const { - assert(0 <= i && i < 3); + btFullAssert(0 <= i && i < 3); return m_el[i]; } @@ -73,41 +75,30 @@ class btMatrix3x3 { void setFromOpenGLSubMatrix(const btScalar *m) { - m_el[0][0] = (m[0]); - m_el[1][0] = (m[1]); - m_el[2][0] = (m[2]); - m_el[0][1] = (m[4]); - m_el[1][1] = (m[5]); - m_el[2][1] = (m[6]); - m_el[0][2] = (m[8]); - m_el[1][2] = (m[9]); - m_el[2][2] = (m[10]); + m_el[0].setValue(m[0],m[4],m[8]); + m_el[1].setValue(m[1],m[5],m[9]); + m_el[2].setValue(m[2],m[6],m[10]); + } void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& zx, const btScalar& zy, const btScalar& zz) { - m_el[0][0] = btScalar(xx); - m_el[0][1] = btScalar(xy); - m_el[0][2] = btScalar(xz); - m_el[1][0] = btScalar(yx); - m_el[1][1] = btScalar(yy); - m_el[1][2] = btScalar(yz); - m_el[2][0] = btScalar(zx); - m_el[2][1] = btScalar(zy); - m_el[2][2] = btScalar(zz); + m_el[0].setValue(xx,xy,xz); + m_el[1].setValue(yx,yy,yz); + m_el[2].setValue(zx,zy,zz); } void setRotation(const btQuaternion& q) { btScalar d = q.length2(); - assert(d != btScalar(0.0)); + btFullAssert(d != btScalar(0.0)); btScalar s = btScalar(2.0) / d; - btScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s; - btScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs; - btScalar xx = q[0] * xs, xy = q[0] * ys, xz = q[0] * zs; - btScalar yy = q[1] * ys, yz = q[1] * zs, zz = q[2] * zs; + btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; + btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; + btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; + btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, xy + wz, btScalar(1.0) - (xx + zz), yz - wx, xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); @@ -168,90 +159,88 @@ class btMatrix3x3 { void getOpenGLSubMatrix(btScalar *m) const { - m[0] = btScalar(m_el[0][0]); - m[1] = btScalar(m_el[1][0]); - m[2] = btScalar(m_el[2][0]); + m[0] = btScalar(m_el[0].x()); + m[1] = btScalar(m_el[1].x()); + m[2] = btScalar(m_el[2].x()); m[3] = btScalar(0.0); - m[4] = btScalar(m_el[0][1]); - m[5] = btScalar(m_el[1][1]); - m[6] = btScalar(m_el[2][1]); + m[4] = btScalar(m_el[0].y()); + m[5] = btScalar(m_el[1].y()); + m[6] = btScalar(m_el[2].y()); m[7] = btScalar(0.0); - m[8] = btScalar(m_el[0][2]); - m[9] = btScalar(m_el[1][2]); - m[10] = btScalar(m_el[2][2]); + m[8] = btScalar(m_el[0].z()); + m[9] = btScalar(m_el[1].z()); + m[10] = btScalar(m_el[2].z()); m[11] = btScalar(0.0); } void getRotation(btQuaternion& q) const { - btScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2]; + btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); + btScalar temp[4]; if (trace > btScalar(0.0)) { btScalar s = btSqrt(trace + btScalar(1.0)); - q[3] = s * btScalar(0.5); + temp[3]=(s * btScalar(0.5)); s = btScalar(0.5) / s; - q[0] = (m_el[2][1] - m_el[1][2]) * s; - q[1] = (m_el[0][2] - m_el[2][0]) * s; - q[2] = (m_el[1][0] - m_el[0][1]) * s; + temp[0]=((m_el[2].y() - m_el[1].z()) * s); + temp[1]=((m_el[0].z() - m_el[2].x()) * s); + temp[2]=((m_el[1].x() - m_el[0].y()) * s); } else { - int i = m_el[0][0] < m_el[1][1] ? - (m_el[1][1] < m_el[2][2] ? 2 : 1) : - (m_el[0][0] < m_el[2][2] ? 2 : 0); + int i = m_el[0].x() < m_el[1].y() ? + (m_el[1].y() < m_el[2].z() ? 2 : 1) : + (m_el[0].x() < m_el[2].z() ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); - q[i] = s * btScalar(0.5); + temp[i] = s * btScalar(0.5); s = btScalar(0.5) / s; - q[3] = (m_el[k][j] - m_el[j][k]) * s; - q[j] = (m_el[j][i] + m_el[i][j]) * s; - q[k] = (m_el[k][i] + m_el[i][k]) * s; + temp[3] = (m_el[k][j] - m_el[j][k]) * s; + temp[j] = (m_el[j][i] + m_el[i][j]) * s; + temp[k] = (m_el[k][i] + m_el[i][k]) * s; } + q.setValue(temp[0],temp[1],temp[2],temp[3]); } - - void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const { - pitch = btScalar(btAsin(-m_el[2][0])); - if (pitch < SIMD_2_PI) + + if (btScalar(m_el[1].z()) < btScalar(1)) { - if (pitch > SIMD_2_PI) + if (btScalar(m_el[1].z()) > -btScalar(1)) { - yaw = btScalar(btAtan2(m_el[1][0], m_el[0][0])); - roll = btScalar(btAtan2(m_el[2][1], m_el[2][2])); + yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); + pitch = btScalar(btAsin(-m_el[1].y())); + roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); } else { - yaw = btScalar(-btAtan2(-m_el[0][1], m_el[0][2])); + yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z())); + pitch = SIMD_HALF_PI; roll = btScalar(0.0); } } else { - yaw = btScalar(btAtan2(-m_el[0][1], m_el[0][2])); + yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z())); + pitch = -SIMD_HALF_PI; roll = btScalar(0.0); } } - - btVector3 getScaling() const - { - return btVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0], - m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1], - m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]); - } + + btMatrix3x3 scaled(const btVector3& s) const { - return btMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2], - m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2], - m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]); + return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), + m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), + m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); } btScalar determinant() const; @@ -263,11 +252,21 @@ class btMatrix3x3 { btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; - btScalar tdot(int c, const btVector3& v) const + SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const + { + return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const + { + return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const { - return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2]; + return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); } + + protected: btScalar cofac(int r1, int c1, int r2, int c2) const { @@ -280,9 +279,9 @@ class btMatrix3x3 { SIMD_FORCE_INLINE btMatrix3x3& btMatrix3x3::operator*=(const btMatrix3x3& m) { - setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]), - m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]), - m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2])); + setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), + m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), + m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); return *this; } @@ -297,17 +296,17 @@ class btMatrix3x3 { btMatrix3x3::absolute() const { return btMatrix3x3( - btFabs(m_el[0][0]), btFabs(m_el[0][1]), btFabs(m_el[0][2]), - btFabs(m_el[1][0]), btFabs(m_el[1][1]), btFabs(m_el[1][2]), - btFabs(m_el[2][0]), btFabs(m_el[2][1]), btFabs(m_el[2][2])); + btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), + btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), + btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); } SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::transpose() const { - return btMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0], - m_el[0][1], m_el[1][1], m_el[2][1], - m_el[0][2], m_el[1][2], m_el[2][2]); + return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), + m_el[0].y(), m_el[1].y(), m_el[2].y(), + m_el[0].z(), m_el[1].z(), m_el[2].z()); } SIMD_FORCE_INLINE btMatrix3x3 @@ -323,26 +322,26 @@ class btMatrix3x3 { { btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); btScalar det = (*this)[0].dot(co); - assert(det != btScalar(0.0f)); - btScalar s = btScalar(1.0f) / det; - return btMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, - co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, - co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); + btFullAssert(det != btScalar(0.0)); + btScalar s = btScalar(1.0) / det; + return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); } SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const { return btMatrix3x3( - m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0], - m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1], - m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2], - m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0], - m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1], - m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2], - m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0], - m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1], - m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]); + m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), + m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), + m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), + m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), + m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), + m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), + m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), + m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), + m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].x()); } SIMD_FORCE_INLINE btMatrix3x3 @@ -365,19 +364,19 @@ class btMatrix3x3 { SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btMatrix3x3& m) { - return btVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v)); + return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); } SIMD_FORCE_INLINE btMatrix3x3 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) { return btMatrix3x3( - m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), - m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), - m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2])); + m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), + m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), + m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); } - +/* SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { return btMatrix3x3( m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], @@ -390,6 +389,7 @@ class btMatrix3x3 { m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); } +*/ #endif diff --git a/extern/bullet2/src/LinearMath/btMotionState.h b/extern/bullet2/src/LinearMath/btMotionState.h index a9e212d3c71..1975e5ff900 100644 --- a/extern/bullet2/src/LinearMath/btMotionState.h +++ b/extern/bullet2/src/LinearMath/btMotionState.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BT_MOTIONSTATE_H #define BT_MOTIONSTATE_H -#include "LinearMath/btTransform.h" +#include "btTransform.h" ///btMotionState allows the dynamics world to synchronize the updated world transforms with graphics ///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation) @@ -34,10 +34,7 @@ class btMotionState //Bullet only calls the update of worldtransform for active objects virtual void setWorldTransform(const btTransform& worldTrans)=0; - //future: when Bullet makes attempt to deactivate object, you can intercept this callback (return false to disable deactivation for this object this frame) - virtual bool deactivationCallback(void* userPointer) { - return true; - } + }; #endif //BT_MOTIONSTATE_H diff --git a/extern/bullet2/src/LinearMath/btPoint3.h b/extern/bullet2/src/LinearMath/btPoint3.h index 4be7e9015bb..a2020e26d12 100644 --- a/extern/bullet2/src/LinearMath/btPoint3.h +++ b/extern/bullet2/src/LinearMath/btPoint3.h @@ -17,7 +17,7 @@ subject to the following restrictions: #ifndef btPoint3_H #define btPoint3_H -#include "LinearMath/btVector3.h" +#include "btVector3.h" typedef btVector3 btPoint3; diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index 4331b668210..961ac484d20 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -16,12 +16,13 @@ subject to the following restrictions: #ifndef SIMD_QUADWORD_H #define SIMD_QUADWORD_H -#include "LinearMath/btScalar.h" +#include "btScalar.h" -ATTRIBUTE_ALIGNED16 (class btQuadWord) +///btQuadWord is base-class for vectors, points +class btQuadWord { protected: btScalar m_x; @@ -31,8 +32,8 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) public: - SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } - SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } +// SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } +// SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; } @@ -40,11 +41,13 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; } - SIMD_FORCE_INLINE void setX(float x) { m_x = x;}; + SIMD_FORCE_INLINE void setX(btScalar x) { m_x = x;}; - SIMD_FORCE_INLINE void setY(float y) { m_y = y;}; + SIMD_FORCE_INLINE void setY(btScalar y) { m_y = y;}; - SIMD_FORCE_INLINE void setZ(float z) { m_z = z;}; + SIMD_FORCE_INLINE void setZ(btScalar z) { m_z = z;}; + + SIMD_FORCE_INLINE void setW(btScalar w) { m_unusedW = w;}; SIMD_FORCE_INLINE const btScalar& x() const { return m_x; } @@ -52,15 +55,18 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) SIMD_FORCE_INLINE const btScalar& z() const { return m_z; } + SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; } + - operator btScalar *() { return &m_x; } - operator const btScalar *() const { return &m_x; } + SIMD_FORCE_INLINE operator btScalar *() { return &m_x; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_x; } SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) { m_x=x; m_y=y; m_z=z; + m_unusedW = 0.f; } /* void getValue(btScalar *m) const @@ -78,15 +84,15 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) m_unusedW=w; } - SIMD_FORCE_INLINE btQuadWord() : - m_x(0.f),m_y(0.f),m_z(0.f),m_unusedW(0.f) + SIMD_FORCE_INLINE btQuadWord() + // :m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.)) { } SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) :m_x(x),m_y(y),m_z(z) //todo, remove this in release/simd ? - ,m_unusedW(0.f) + ,m_unusedW(btScalar(0.)) { } diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index aec25a54955..d5a7913b742 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -17,7 +17,7 @@ subject to the following restrictions: #ifndef SIMD__QUATERNION_H_ #define SIMD__QUATERNION_H_ -#include "LinearMath/btVector3.h" +#include "btVector3.h" class btQuaternion : public btQuadWord { public: @@ -68,13 +68,13 @@ public: btQuaternion& operator+=(const btQuaternion& q) { - m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3]; + m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW; return *this; } btQuaternion& operator-=(const btQuaternion& q) { - m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3]; + m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW; return *this; } @@ -87,16 +87,16 @@ public: btQuaternion& operator*=(const btQuaternion& q) { - setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(), - m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(), - m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(), - m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z()); + setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(), + m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(), + m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(), + m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z()); return *this; } btScalar dot(const btQuaternion& q) const { - return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3]; + return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW; } btScalar length2() const @@ -150,7 +150,7 @@ public: btScalar getAngle() const { - btScalar s = 2.f * btAcos(m_unusedW); + btScalar s = btScalar(2.) * btAcos(m_unusedW); return s; } @@ -165,20 +165,20 @@ public: operator+(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]); + return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW); } SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]); + return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW); } SIMD_FORCE_INLINE btQuaternion operator-() const { const btQuaternion& q2 = *this; - return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2[3]); + return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_unusedW); } SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const @@ -202,7 +202,7 @@ public: return btQuaternion((m_x * s0 + q.x() * s1) * d, (m_y * s0 + q.y() * s1) * d, (m_z * s0 + q.z() * s1) * d, - (m_unusedW * s0 + q[3] * s1) * d); + (m_unusedW * s0 + q.m_unusedW * s1) * d); } else { @@ -219,7 +219,7 @@ public: SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q) { - return btQuaternion(-q.x(), -q.y(), -q.z(), -q[3]); + return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w()); } @@ -227,27 +227,27 @@ operator-(const btQuaternion& q) SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q1, const btQuaternion& q2) { - return btQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(), - q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(), - q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(), - q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); + return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), + q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), + q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), + q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); } SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q, const btVector3& w) { - return btQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(), - q[3] * w.y() + q.z() * w.x() - q.x() * w.z(), - q[3] * w.z() + q.x() * w.y() - q.y() * w.x(), + return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), + q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), + q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); } SIMD_FORCE_INLINE btQuaternion operator*(const btVector3& w, const btQuaternion& q) { - return btQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(), - w.y() * q[3] + w.z() * q.x() - w.x() * q.z(), - w.z() * q[3] + w.x() * q.y() - w.y() * q.x(), + return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), + w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), + w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); } diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index 4d013a65853..37a0c8c3be5 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: // Note: We must declare these private static variables again here to // avoid link errors. bool btProfiler::mEnabled = false; -hidden::Clock btProfiler::mClock; +btClock btProfiler::mClock; unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0; unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0; std::map<std::string, hidden::ProfileBlock*> btProfiler::mProfileBlocks; diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index ec560c8e33e..9389fcd8156 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -22,16 +22,16 @@ subject to the following restrictions: #ifndef QUICK_PROF_H #define QUICK_PROF_H -#define USE_QUICKPROF 1 +#include "btScalar.h" -#ifdef USE_QUICKPROF +//#define USE_QUICKPROF 1 +//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes. -#include <iostream> -#include <fstream> -#include <string> -#include <map> +//if you don't need btClock, you can comment next line +#define USE_BT_CLOCK 1 +#ifdef USE_BT_CLOCK #ifdef __PPU__ #include <sys/sys_time.h> #include <stdio.h> @@ -43,7 +43,12 @@ typedef uint64_t __int64; #endif #if defined(WIN32) || defined(_WIN32) - #define USE_WINDOWS_TIMERS + + #define USE_WINDOWS_TIMERS + #define WIN32_LEAN_AND_MEAN + #define NOWINRES + #define NOMCX + #define NOIME #include <windows.h> #include <time.h> #else @@ -51,39 +56,12 @@ typedef uint64_t __int64; #endif #define mymin(a,b) (a > b ? a : b) -namespace hidden -{ - /// A simple data structure representing a single timed block - /// of code. - struct ProfileBlock - { - ProfileBlock() - { - currentBlockStartMicroseconds = 0; - currentCycleTotalMicroseconds = 0; - lastCycleTotalMicroseconds = 0; - totalMicroseconds = 0; - } - /// The starting time (in us) of the current block update. - unsigned long int currentBlockStartMicroseconds; - - /// The accumulated time (in us) spent in this block during the - /// current profiling cycle. - unsigned long int currentCycleTotalMicroseconds; - - /// The accumulated time (in us) spent in this block during the - /// past profiling cycle. - unsigned long int lastCycleTotalMicroseconds; - - /// The total accumulated time (in us) spent in this block. - unsigned long int totalMicroseconds; - }; - - class Clock +/// basic clock +class btClock { public: - Clock() + btClock() { #ifdef USE_WINDOWS_TIMERS QueryPerformanceFrequency(&mClockFrequency); @@ -91,7 +69,7 @@ namespace hidden reset(); } - ~Clock() + ~btClock() { } @@ -111,14 +89,14 @@ namespace hidden __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); mStartTime = newTime; #else - gettimeofday(&mStartTime, NULL); + gettimeofday(&mStartTime, 0); #endif #endif } /// Returns the time in ms since the last call to reset or since - /// the Clock was created. + /// the btClock was created. unsigned long int getTimeMilliseconds() { #ifdef USE_WINDOWS_TIMERS @@ -168,7 +146,7 @@ namespace hidden #else struct timeval currentTime; - gettimeofday(¤tTime, NULL); + gettimeofday(¤tTime, 0); return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + (currentTime.tv_usec - mStartTime.tv_usec) / 1000; #endif //__PPU__ @@ -226,7 +204,7 @@ namespace hidden #else struct timeval currentTime; - gettimeofday(¤tTime, NULL); + gettimeofday(¤tTime, 0); return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + (currentTime.tv_usec - mStartTime.tv_usec); #endif//__PPU__ @@ -248,6 +226,50 @@ namespace hidden #endif //__PPU__ }; + +#endif //USE_BT_CLOCK + + +#ifdef USE_QUICKPROF + + +#include <iostream> +#include <fstream> +#include <string> +#include <map> + + + + +namespace hidden +{ + /// A simple data structure representing a single timed block + /// of code. + struct ProfileBlock + { + ProfileBlock() + { + currentBlockStartMicroseconds = 0; + currentCycleTotalMicroseconds = 0; + lastCycleTotalMicroseconds = 0; + totalMicroseconds = 0; + } + + /// The starting time (in us) of the current block update. + unsigned long int currentBlockStartMicroseconds; + + /// The accumulated time (in us) spent in this block during the + /// current profiling cycle. + unsigned long int currentCycleTotalMicroseconds; + + /// The accumulated time (in us) spent in this block during the + /// past profiling cycle. + unsigned long int lastCycleTotalMicroseconds; + + /// The total accumulated time (in us) spent in this block. + unsigned long int totalMicroseconds; + }; + }; /// A static class that manages timing for a set of profiling blocks. @@ -336,6 +358,7 @@ public: /// Prints an error message to standard output. inline static void printError(const std::string& msg) { + //btAssert(0); std::cout << "[QuickProf error] " << msg << std::endl; } @@ -343,7 +366,7 @@ public: static bool mEnabled; /// The clock used to time profile blocks. - static hidden::Clock mClock; + static btClock mClock; /// The starting time (in us) of the current profiling cycle. static unsigned long int mCurrentCycleStartMicroseconds; diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index dd76fb2de1a..222315c1c74 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -25,10 +25,11 @@ subject to the following restrictions: #ifdef WIN32 - #if defined(__MINGW32__) || defined(__CYGWIN__) + #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a #else + #define BT_HAS_ALIGNED_ALOCATOR #pragma warning(disable:4530) #pragma warning(disable:4996) #pragma warning(disable:4786) @@ -38,8 +39,21 @@ subject to the following restrictions: #include <assert.h> #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) #else +#if defined (__CELLOS_LV2__) + #define SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #ifndef assert + #include <assert.h> + #endif + #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) +#else + //non-windows systems #define SIMD_FORCE_INLINE inline @@ -48,16 +62,28 @@ subject to the following restrictions: #include <assert.h> #endif #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) +#endif //__CELLOS_LV2__ #endif +/// older compilers (gcc 3.x) and Sun needs double version of sqrt etc. +/// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor) +#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) +//use slow double float precision operation on those platforms +#ifndef BT_USE_DOUBLE_PRECISION +#define BT_FORCE_DOUBLE_FUNCTIONS +#endif +#endif +#if defined(BT_USE_DOUBLE_PRECISION) +typedef double btScalar; +#else +typedef float btScalar; +#endif -typedef float btScalar; -///older compilers (gcc 3.x) and Sun needs double versions of srqt etc. -///exclude Apple Intel (it's assumed to be a Macbook or newer Intel Dual Core processor) -#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) -//use slow double float precision operation on those platforms +#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } @@ -90,13 +116,19 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } #endif -#define SIMD_2_PI 6.283185307179586232f -#define SIMD_PI (SIMD_2_PI * btScalar(0.5f)) -#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25f)) -#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0f)) -#define SIMD_DEGS_PER_RAD (btScalar(360.0f) / SIMD_2_PI) +#define SIMD_2_PI btScalar(6.283185307179586232) +#define SIMD_PI (SIMD_2_PI * btScalar(0.5)) +#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25)) +#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) +#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) + +#ifdef BT_USE_DOUBLE_PRECISION +#define SIMD_EPSILON DBL_EPSILON +#define SIMD_INFINITY DBL_MAX +#else #define SIMD_EPSILON FLT_EPSILON #define SIMD_INFINITY FLT_MAX +#endif SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } @@ -117,7 +149,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y) */ SIMD_FORCE_INLINE int btIsNegative(btScalar x) { - return x < 0.0f ? 1 : 0; + return x < btScalar(0.0) ? 1 : 0; } SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } diff --git a/extern/bullet2/src/LinearMath/btSimdMinMax.h b/extern/bullet2/src/LinearMath/btSimdMinMax.h index 2731c8b09f3..75e83f3c53f 100644 --- a/extern/bullet2/src/LinearMath/btSimdMinMax.h +++ b/extern/bullet2/src/LinearMath/btSimdMinMax.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SIMD_MINMAX_H #define SIMD_MINMAX_H -#include "LinearMath/btScalar.h" +#include "btScalar.h" template <class T> SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index 3f9a48407c7..b1f3dfca492 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -17,8 +17,8 @@ subject to the following restrictions: #ifndef btTransform_H #define btTransform_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" +#include "btVector3.h" +#include "btMatrix3x3.h" ///btTransform supports rigid transforms (only translation and rotation, no scaling/shear) @@ -48,17 +48,19 @@ public: m_origin = t1(t2.m_origin); } - void multInverseLeft(const btTransform& t1, const btTransform& t2) { +/* void multInverseLeft(const btTransform& t1, const btTransform& t2) { btVector3 v = t2.m_origin - t1.m_origin; m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis); m_origin = v * t1.m_basis; } + */ + SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const { - return btVector3(m_basis[0].dot(x) + m_origin[0], - m_basis[1].dot(x) + m_origin[1], - m_basis[2].dot(x) + m_origin[2]); + return btVector3(m_basis[0].dot(x) + m_origin.x(), + m_basis[1].dot(x) + m_origin.y(), + m_basis[2].dot(x) + m_origin.z()); } SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const @@ -88,18 +90,16 @@ public: void setFromOpenGLMatrix(const btScalar *m) { m_basis.setFromOpenGLSubMatrix(m); - m_origin[0] = m[12]; - m_origin[1] = m[13]; - m_origin[2] = m[14]; + m_origin.setValue(m[12],m[13],m[14]); } void getOpenGLMatrix(btScalar *m) const { m_basis.getOpenGLSubMatrix(m); - m[12] = m_origin[0]; - m[13] = m_origin[1]; - m[14] = m_origin[2]; - m[15] = btScalar(1.0f); + m[12] = m_origin.x(); + m[13] = m_origin.y(); + m[14] = m_origin.z(); + m[15] = btScalar(1.0); } SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 39fa830f4df..bc42fd166b6 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -16,48 +16,40 @@ subject to the following restrictions: #ifndef SIMD_TRANSFORM_UTIL_H #define SIMD_TRANSFORM_UTIL_H -#include "LinearMath/btTransform.h" -#define ANGULAR_MOTION_THRESHOLD 0.5f*SIMD_HALF_PI +#include "btTransform.h" +#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) -#define btRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */ +#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) { - return btVector3(supportDir.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), - supportDir.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - supportDir.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); } inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) { - if (btFabs(n[2]) > SIMDSQRT12) { + if (btFabs(n.z()) > SIMDSQRT12) { // choose p in y-z plane btScalar a = n[1]*n[1] + n[2]*n[2]; btScalar k = btRecipSqrt (a); - p[0] = 0; - p[1] = -n[2]*k; - p[2] = n[1]*k; + p.setValue(0,-n[2]*k,n[1]*k); // set q = n x p - q[0] = a*k; - q[1] = -n[0]*p[2]; - q[2] = n[0]*p[1]; + q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); } else { // choose p in x-y plane - btScalar a = n[0]*n[0] + n[1]*n[1]; + btScalar a = n.x()*n.x() + n.y()*n.y(); btScalar k = btRecipSqrt (a); - p[0] = -n[1]*k; - p[1] = n[0]*k; - p[2] = 0; + p.setValue(-n.y()*k,n.x()*k,0); // set q = n x p - q[0] = -n[2]*p[1]; - q[1] = n[2]*p[0]; - q[2] = a*k; + q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); } } @@ -74,9 +66,9 @@ public: predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); // #define QUATERNION_DERIVATIVE #ifdef QUATERNION_DERIVATIVE - btQuaternion orn = curTrans.getRotation(); - orn += (angvel * orn) * (timeStep * 0.5f); - orn.normalize(); + btQuaternion predictedOrn = curTrans.getRotation(); + predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); + predictedOrn.normalize(); #else //exponential map btVector3 axis; @@ -87,20 +79,21 @@ public: fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } - if ( fAngle < 0.001f ) + if ( fAngle < btScalar(0.001) ) { // use Taylor's expansions of sync function - axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle ); + axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle ); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(0.5f*fAngle*timeStep)/fAngle ); + axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle ); } - btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*0.5f )); + btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) )); btQuaternion orn0 = curTrans.getRotation(); btQuaternion predictedOrn = dorn * orn0; + predictedOrn.normalize(); #endif predictedTransform.setRotation(predictedOrn); } @@ -130,11 +123,11 @@ public: angle = dorn.getAngle(); axis = btVector3(dorn.x(),dorn.y(),dorn.z()); - axis[3] = 0.f; + axis[3] = btScalar(0.); //check for axis length btScalar len = axis.length2(); if (len < SIMD_EPSILON*SIMD_EPSILON) - axis = btVector3(1.f,0.f,0.f); + axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); else axis /= btSqrt(len); } diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 5a35652ecd3..74d41ad2a19 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -19,11 +19,10 @@ subject to the following restrictions: #include "btQuadWord.h" - -///btVector3 is 16byte aligned, and has an extra unused component m_w -///this extra component can be used by derived classes (Quaternion?) or by user -class btVector3 : public btQuadWord { - +///btVector3 can be used to represent 3D points and vectors. +///It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user +///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers +class btVector3 : public btQuadWord { public: SIMD_FORCE_INLINE btVector3() {} @@ -31,7 +30,7 @@ public: SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) - :btQuadWord(x,y,z,0.f) + :btQuadWord(x,y,z,btScalar(0.)) { } @@ -64,7 +63,7 @@ public: SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) { - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } @@ -99,7 +98,7 @@ public: SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const { btScalar s = btSqrt(length2() * v.length2()); - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return btAcos(dot(v) / s); } @@ -148,10 +147,10 @@ public: SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) { - btScalar s = 1.0f - rt; - m_x = s * v0[0] + rt * v1.x(); - m_y = s * v0[1] + rt * v1.y(); - m_z = s * v0[2] + rt * v1.z(); + btScalar s = btScalar(1.0) - rt; + m_x = s * v0.x() + rt * v1.x(); + m_y = s * v0.y() + rt * v1.y(); + m_z = s * v0.z() + rt * v1.z(); //don't do the unused w component // m_co[3] = s * v0[3] + rt * v1[3]; } @@ -213,7 +212,7 @@ operator*(const btScalar& s, const btVector3& v) SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v, const btScalar& s) { - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return v * (btScalar(1.0) / s); } @@ -271,7 +270,7 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) { - return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2]; + return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z(); } SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const @@ -327,13 +326,13 @@ public: - float getW() const { return m_unusedW;} + btScalar getW() const { return m_unusedW;} SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; - float maxVal = -1e30f; + btScalar maxVal = btScalar(-1e30); if (m_x > maxVal) { maxIndex = 0; @@ -366,7 +365,7 @@ public: SIMD_FORCE_INLINE int minAxis4() const { int minIndex = -1; - float minVal = 1e30f; + btScalar minVal = btScalar(1e30); if (m_x < minVal) { minIndex = 0; diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet2/src/btBulletCollisionCommon.h index 834dca09e47..8417ccc671f 100644 --- a/extern/bullet2/src/btBulletCollisionCommon.h +++ b/extern/bullet2/src/btBulletCollisionCommon.h @@ -25,6 +25,7 @@ subject to the following restrictions: ///Collision Shapes #include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btCylinderShape.h" #include "BulletCollision/CollisionShapes/btConeShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" |