diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision')
183 files changed, 28415 insertions, 3109 deletions
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index be4a11506df..d7eea33ea41 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -21,640 +21,18 @@ #include <assert.h> -#ifdef DEBUG_BROADPHASE -#include <stdio.h> -void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality) +btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache) +:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache) { - 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) -{ - (void)shapeType; - BP_FP_INT_TYPE handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask); - - Handle* handle = getHandle(handleId); - - return handle; -} - -void btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy) -{ - Handle* handle = static_cast<Handle*>(proxy); - removeHandle(handle->m_handleId); -} - -void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax) -{ - Handle* handle = static_cast<Handle*>(proxy); - updateHandle(handle->m_handleId,aabbMin,aabbMax); - -} - - - - - - -btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles) -:btOverlappingPairCache() -{ - m_invalidPair = 0; - //assert(bounds.HasVolume()); - // 1 handle is reserved as sentinel - btAssert(maxHandles > 1 && maxHandles < BP_MAX_HANDLES); - - // init bounds - m_worldAabbMin = worldAabbMin; - m_worldAabbMax = worldAabbMax; - - btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; - - 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]; - m_maxHandles = maxHandles; - m_numHandles = 0; - - // handle 0 is reserved as the null index, and is also used as the sentinel - m_firstFreeHandle = 1; - { - for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) - m_pHandles[i].SetNextFree(i + 1); - m_pHandles[maxHandles - 1].SetNextFree(0); - } - - { - // allocate edge buffers - for (int i = 0; i < 3; i++) - m_pEdges[i] = new Edge[maxHandles * 2]; - } - //removed overlap management - - // make boundary sentinels - - m_pHandles[0].m_clientObject = 0; - - for (int axis = 0; axis < 3; axis++) - { - m_pHandles[0].m_minEdges[axis] = 0; - m_pHandles[0].m_maxEdges[axis] = 1; - - m_pEdges[axis][0].m_pos = 0; - m_pEdges[axis][0].m_handle = 0; - 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() -{ - - for (int i = 2; i >= 0; i--) - delete[] m_pEdges[i]; - delete[] m_pHandles; -} - -void btAxisSweep3::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const -{ - btPoint3 clampedPoint(point); - - - - clampedPoint.setMax(m_worldAabbMin); - clampedPoint.setMin(m_worldAabbMax); - - btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; - 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); - -} - - - -BP_FP_INT_TYPE btAxisSweep3::allocHandle() -{ - assert(m_firstFreeHandle); - - BP_FP_INT_TYPE handle = m_firstFreeHandle; - m_firstFreeHandle = getHandle(handle)->GetNextFree(); - m_numHandles++; - - return handle; -} - -void btAxisSweep3::freeHandle(BP_FP_INT_TYPE handle) -{ - assert(handle > 0 && handle < m_maxHandles); - - getHandle(handle)->SetNextFree(m_firstFreeHandle); - m_firstFreeHandle = handle; - - m_numHandles--; -} - - - -BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask) -{ - // quantize the bounds - BP_FP_INT_TYPE min[3], max[3]; - quantize(min, aabbMin, 0); - quantize(max, aabbMax, 1); - - // allocate a handle - BP_FP_INT_TYPE handle = allocHandle(); - assert(handle!= 0xcdcd); - - Handle* pHandle = getHandle(handle); - - pHandle->m_handleId = handle; - //pHandle->m_pOverlaps = 0; - pHandle->m_clientObject = pOwner; - pHandle->m_collisionFilterGroup = collisionFilterGroup; - pHandle->m_collisionFilterMask = collisionFilterMask; - - // compute current limit of edge arrays - BP_FP_INT_TYPE limit = m_numHandles * 2; - - - // insert new edges just inside the max boundary edge - 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]; - - m_pEdges[axis][limit - 1].m_pos = min[axis]; - m_pEdges[axis][limit - 1].m_handle = handle; - - m_pEdges[axis][limit].m_pos = max[axis]; - m_pEdges[axis][limit].m_handle = handle; - - pHandle->m_minEdges[axis] = limit - 1; - pHandle->m_maxEdges[axis] = limit; - } - - // now sort the new edges to their correct position - sortMinDown(0, pHandle->m_minEdges[0], false); - sortMaxDown(0, pHandle->m_maxEdges[0], false); - sortMinDown(1, pHandle->m_minEdges[1], false); - sortMaxDown(1, pHandle->m_maxEdges[1], false); - sortMinDown(2, pHandle->m_minEdges[2], true); - sortMaxDown(2, pHandle->m_maxEdges[2], true); - - - return handle; -} - - -void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle) -{ - - Handle* pHandle = getHandle(handle); - - //explicitly remove the pairs containing the proxy - //we could do it also in the sortMinUp (passing true) - //todo: compare performance - removeOverlappingPairsContainingProxy(pHandle); - - - // compute current limit of edge arrays - int limit = m_numHandles * 2; - - int axis; - - for (axis = 0;axis<3;axis++) - { - 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]; - BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; - pEdges[max].m_pos = BP_HANDLE_SENTINEL; - - sortMaxUp(axis,max,false); - - - 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 = 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 - - for (int axis = 0; axis < 3; axis++) - { - if (axis != ignoreAxis) - { - if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || - pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) - { - return false; - } - } - } - - //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization) - - /*for (int axis = 0; axis < 3; axis++) - { - if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos || - m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos) - { - return false; - } - } - */ - - return true; -} - -void btAxisSweep3::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax) -{ -// assert(bounds.IsFinite()); - //assert(bounds.HasVolume()); - - Handle* pHandle = getHandle(handle); - - // quantize the new bounds - 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++) - { - 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; - - m_pEdges[axis][emin].m_pos = min[axis]; - m_pEdges[axis][emax].m_pos = max[axis]; - - // expand (only adds overlaps) - if (dmin < 0) - sortMinDown(axis, emin); - - if (dmax > 0) - sortMaxUp(axis, emax); - - // shrink (only removes overlaps) - if (dmin > 0) - sortMinUp(axis, emin); - - if (dmax < 0) - sortMaxDown(axis, emax); - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - } - - -} - - - - -// sorting a min edge downwards can only ever *add* overlaps -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); - - while (pEdge->m_pos < pPrev->m_pos) - { - Handle* pHandlePrev = getHandle(pPrev->m_handle); - - if (pPrev->IsMax()) - { - // if previous edge is a maximum check the bounds and add an overlap if necessary - if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev)) - { - addOverlappingPair(pHandleEdge,pHandlePrev); - - //AddOverlap(pEdge->m_handle, pPrev->m_handle); - - } - - // update edge reference in other handle - pHandlePrev->m_maxEdges[axis]++; - } - else - pHandlePrev->m_minEdges[axis]++; - - pHandleEdge->m_minEdges[axis]--; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pPrev; - *pPrev = swap; - - // decrement - 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, BP_FP_INT_TYPE edge, bool updateOverlaps) -{ - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pNext = pEdge + 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) - { - Handle* pHandleNext = getHandle(pNext->m_handle); - - if (pNext->IsMax()) - { - // 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); - */ - - } - - // update edge reference in other handle - pHandleNext->m_maxEdges[axis]--; - } - else - pHandleNext->m_minEdges[axis]--; - - pHandleEdge->m_minEdges[axis]++; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pNext; - *pNext = swap; - - // increment - pEdge++; - pNext++; - } - + btAssert(maxHandles > 1 && maxHandles < 32767); } -// sorting a max edge downwards can only ever *remove* overlaps -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); - - while (pEdge->m_pos < pPrev->m_pos) - { - Handle* pHandlePrev = getHandle(pPrev->m_handle); - - if (!pPrev->IsMax()) - { - // 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); - //assert(pair); - - if (pair) - { - removeOverlappingPair(*pair); - } - */ - - } - - // update edge reference in other handle - pHandlePrev->m_minEdges[axis]++;; - } - else - pHandlePrev->m_maxEdges[axis]++; - - pHandleEdge->m_maxEdges[axis]--; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pPrev; - *pPrev = swap; - // decrement - 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, BP_FP_INT_TYPE edge, bool updateOverlaps) +bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache ) +:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache) { - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pNext = pEdge + 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) - { - Handle* pHandleNext = getHandle(pNext->m_handle); - - if (!pNext->IsMax()) - { - // if next edge is a minimum check the bounds and add an overlap if necessary - if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext)) - { - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pNext->m_handle); - addOverlappingPair(handle0,handle1); - } - - // update edge reference in other handle - pHandleNext->m_minEdges[axis]--; - } - else - pHandleNext->m_maxEdges[axis]--; - - pHandleEdge->m_maxEdges[axis]++; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pNext; - *pNext = swap; - - // increment - pEdge++; - pNext++; - } - + // 1 handle is reserved as sentinel + btAssert(maxHandles > 1 && maxHandles < 2147483647); } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index 57bbb368672..d0ad09a385a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -19,34 +19,26 @@ #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 "btBroadphaseInterface.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 +#include "btOverlappingPairCallback.h" //#define DEBUG_BROADPHASE 1 +#define USE_OVERLAP_TEST_ON_REMOVES 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 -class btAxisSweep3 : public btOverlappingPairCache +/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase. +/// It uses quantized integers to represent the begin and end points for each of the 3 axis. +/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead. +template <typename BP_FP_INT_TYPE> +class btAxisSweep3Internal : public btBroadphaseInterface { +protected: + + BP_FP_INT_TYPE m_bpHandleMask; + BP_FP_INT_TYPE m_handleSentinel; public: @@ -57,47 +49,57 @@ public: BP_FP_INT_TYPE m_pos; // low bit is min/max BP_FP_INT_TYPE m_handle; - BP_FP_INT_TYPE IsMax() const {return m_pos & 1;} + BP_FP_INT_TYPE IsMax() const {return static_cast<BP_FP_INT_TYPE>(m_pos & 1);} }; public: - class Handle : public btBroadphaseProxy + class Handle : public btBroadphaseProxy { public: - + BT_DECLARE_ALIGNED_ALLOCATOR(); + // indexes into the edge arrays BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 - BP_FP_INT_TYPE m_handleId; +// BP_FP_INT_TYPE m_uniqueId; BP_FP_INT_TYPE m_pad; //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject - inline void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} - inline BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} + SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} + SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry -private: +protected: btPoint3 m_worldAabbMin; // overall system bounds btPoint3 m_worldAabbMax; // overall system bounds btVector3 m_quantize; // scaling factor for quantization BP_FP_INT_TYPE m_numHandles; // number of active handles - int m_maxHandles; // max number of handles + BP_FP_INT_TYPE m_maxHandles; // max number of handles Handle* m_pHandles; // handles pool + 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) + void* m_pEdgesRawPtr[3]; + + btOverlappingPairCache* m_pairCache; + + ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. + btOverlappingPairCallback* m_userPairCallback; + + bool m_ownsPairCache; - int m_invalidPair; + int m_invalidPair; // allocation/deallocation BP_FP_INT_TYPE allocHandle(); void freeHandle(BP_FP_INT_TYPE handle); - bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB); + bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1); #ifdef DEBUG_BROADPHASE void debugPrintAxis(int axis,bool checkCardinality=true); @@ -108,29 +110,803 @@ private: 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); + void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); public: - btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384); - virtual ~btAxisSweep3(); - virtual void refreshOverlappingPairs(); + btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0); + + virtual ~btAxisSweep3Internal(); + + BP_FP_INT_TYPE getNumHandles() const + { + return m_numHandles; + } + + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - 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;} + BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher); + void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher); + SIMD_FORCE_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); + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); + + bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + btOverlappingPairCache* getOverlappingPairCache() + { + return m_pairCache; + } + const btOverlappingPairCache* getOverlappingPairCache() const + { + return m_pairCache; + } + + void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback) + { + m_userPairCallback = pairCallback; + } + const btOverlappingPairCallback* getOverlappingPairUserCallback() const + { + return m_userPairCallback; + } + + ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame + ///will add some transform later + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin = m_worldAabbMin; + aabbMax = m_worldAabbMax; + } + + virtual void printStats() + { +/* printf("btAxisSweep3.h\n"); + printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); + printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(), + m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ()); + */ + + } + +}; + +//////////////////////////////////////////////////////////////////// + + + + +#ifdef DEBUG_BROADPHASE +#include <stdio.h> + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3<BP_FP_INT_TYPE>::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 + +template <typename BP_FP_INT_TYPE> +btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) +{ + (void)shapeType; + BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy); + + Handle* handle = getHandle(handleId); + + return handle; +} + + + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + Handle* handle = static_cast<Handle*>(proxy); + removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher); +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) +{ + Handle* handle = static_cast<Handle*>(proxy); + updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); + +} + + + + + +template <typename BP_FP_INT_TYPE> +btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache ) +:m_bpHandleMask(handleMask), +m_handleSentinel(handleSentinel), +m_pairCache(pairCache), +m_userPairCallback(0), +m_ownsPairCache(false), +m_invalidPair(0) +{ + BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle + + if (!m_pairCache) + { + void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16); + m_pairCache = new(ptr) btHashedOverlappingPairCache(); + m_ownsPairCache = true; + } + + //assert(bounds.HasVolume()); + + // init bounds + m_worldAabbMin = worldAabbMin; + m_worldAabbMax = worldAabbMax; + + btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; + + BP_FP_INT_TYPE maxInt = m_handleSentinel; + + m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; + + // allocate handles buffer, using btAlignedAlloc, and put all handles on free list + m_pHandles = new Handle[maxHandles]; + + m_maxHandles = maxHandles; + m_numHandles = 0; + + // handle 0 is reserved as the null index, and is also used as the sentinel + m_firstFreeHandle = 1; + { + for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) + m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1)); + m_pHandles[maxHandles - 1].SetNextFree(0); + } + + { + // allocate edge buffers + for (int i = 0; i < 3; i++) + { + m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16); + m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2]; + } + } + //removed overlap management + + // make boundary sentinels + + m_pHandles[0].m_clientObject = 0; + + for (int axis = 0; axis < 3; axis++) + { + m_pHandles[0].m_minEdges[axis] = 0; + m_pHandles[0].m_maxEdges[axis] = 1; + + m_pEdges[axis][0].m_pos = 0; + m_pEdges[axis][0].m_handle = 0; + m_pEdges[axis][1].m_pos = m_handleSentinel; + m_pEdges[axis][1].m_handle = 0; +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + + } + +} + +template <typename BP_FP_INT_TYPE> +btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal() +{ + + for (int i = 2; i >= 0; i--) + { + btAlignedFree(m_pEdgesRawPtr[i]); + } + delete [] m_pHandles; + + if (m_ownsPairCache) + { + m_pairCache->~btOverlappingPairCache(); + btAlignedFree(m_pairCache); + } +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const +{ + btPoint3 clampedPoint(point); + + + + clampedPoint.setMax(m_worldAabbMin); + clampedPoint.setMin(m_worldAabbMax); + + btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; + out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax); + out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax); + out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax); + +} + + +template <typename BP_FP_INT_TYPE> +BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle() +{ + assert(m_firstFreeHandle); + + BP_FP_INT_TYPE handle = m_firstFreeHandle; + m_firstFreeHandle = getHandle(handle)->GetNextFree(); + m_numHandles++; + + return handle; +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle) +{ + assert(handle > 0 && handle < m_maxHandles); + + getHandle(handle)->SetNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + + m_numHandles--; +} + + +template <typename BP_FP_INT_TYPE> +BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) +{ + // quantize the bounds + BP_FP_INT_TYPE min[3], max[3]; + quantize(min, aabbMin, 0); + quantize(max, aabbMax, 1); + + // allocate a handle + BP_FP_INT_TYPE handle = allocHandle(); + + + Handle* pHandle = getHandle(handle); + + pHandle->m_uniqueId = static_cast<int>(handle); + //pHandle->m_pOverlaps = 0; + pHandle->m_clientObject = pOwner; + pHandle->m_collisionFilterGroup = collisionFilterGroup; + pHandle->m_collisionFilterMask = collisionFilterMask; + pHandle->m_multiSapParentProxy = multiSapProxy; + + // compute current limit of edge arrays + BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2); + + + // insert new edges just inside the max boundary edge + 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]; + + m_pEdges[axis][limit - 1].m_pos = min[axis]; + m_pEdges[axis][limit - 1].m_handle = handle; + + m_pEdges[axis][limit].m_pos = max[axis]; + m_pEdges[axis][limit].m_handle = handle; + + pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1); + pHandle->m_maxEdges[axis] = limit; + } + + // now sort the new edges to their correct position + sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false); + sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false); + sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false); + sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false); + sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true); + sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true); + + + return handle; +} + + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher) +{ + + Handle* pHandle = getHandle(handle); + + //explicitly remove the pairs containing the proxy + //we could do it also in the sortMinUp (passing true) + //todo: compare performance + if (!m_pairCache->hasDeferredRemoval()) + { + m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); + } + + // compute current limit of edge arrays + int limit = static_cast<int>(m_numHandles * 2); + + int axis; + + for (axis = 0;axis<3;axis++) + { + 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]; + BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; + pEdges[max].m_pos = m_handleSentinel; + + sortMaxUp(axis,max,dispatcher,false); + + + BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; + pEdges[i].m_pos = m_handleSentinel; + + + sortMinUp(axis,i,dispatcher,false); + + pEdges[limit-1].m_handle = 0; + pEdges[limit-1].m_pos = m_handleSentinel; + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis,false); +#endif //DEBUG_BROADPHASE + + + } + + + // free the handle + freeHandle(handle); + + +} + +extern int gOverlappingPairs; +//#include <stdio.h> + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher) +{ + + if (m_pairCache->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;i<overlappingPairArray.size();i++) + { + + btBroadphasePair& pair = overlappingPairArray[i]; + + bool isDuplicate = (pair == previousPair); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); + + if (hasOverlap) + { + needsRemoval = false;//callback->processOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + m_pairCache->cleanOverlappingPair(pair,dispatcher); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + m_invalidPair++; + gOverlappingPairs--; + } + + } + + ///if you don't like to skip the invalid pairs in the array, execute following code: + #define CLEAN_INVALID_PAIRS 1 + #ifdef CLEAN_INVALID_PAIRS + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + #endif//CLEAN_INVALID_PAIRS + + //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); + } + + + + + +} + + +template <typename BP_FP_INT_TYPE> +bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(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; +} + +template <typename BP_FP_INT_TYPE> +bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1) +{ + //optimization 1: check the array index (memory address), instead of the m_pos + + if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || + pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] || + pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] || + pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) + { + return false; + } + return true; +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher) +{ +// assert(bounds.IsFinite()); + //assert(bounds.HasVolume()); + + Handle* pHandle = getHandle(handle); + + // quantize the new bounds + 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++) + { + 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; + + m_pEdges[axis][emin].m_pos = min[axis]; + m_pEdges[axis][emax].m_pos = max[axis]; + + // expand (only adds overlaps) + if (dmin < 0) + sortMinDown(axis, emin,dispatcher,true); + + if (dmax > 0) + sortMaxUp(axis, emax,dispatcher,true); + + // shrink (only removes overlaps) + if (dmin > 0) + sortMinUp(axis, emin,dispatcher,true); + + if (dmax < 0) + sortMaxDown(axis, emax,dispatcher,true); + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } + + +} + + + + +// sorting a min edge downwards can only ever *add* overlaps +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) +{ + + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pPrev = pEdge - 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos < pPrev->m_pos) + { + Handle* pHandlePrev = getHandle(pPrev->m_handle); + + if (pPrev->IsMax()) + { + // if previous edge is a maximum check the bounds and add an overlap if necessary + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2)) + { + m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev); + if (m_userPairCallback) + m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev); + + //AddOverlap(pEdge->m_handle, pPrev->m_handle); + + } + + // update edge reference in other handle + pHandlePrev->m_maxEdges[axis]++; + } + else + pHandlePrev->m_minEdges[axis]++; + + pHandleEdge->m_minEdges[axis]--; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pPrev; + *pPrev = swap; + + // decrement + pEdge--; + pPrev--; + } + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + +} + +// sorting a min edge upwards can only ever *remove* overlaps +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pNext = pEdge + 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) + { + Handle* pHandleNext = getHandle(pNext->m_handle); + + if (pNext->IsMax()) + { + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pNext->m_handle); + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + + // if next edge is maximum remove any overlap between the two handles + if (updateOverlaps +#ifdef USE_OVERLAP_TEST_ON_REMOVES + && testOverlap2D(handle0,handle1,axis1,axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) + { + + + m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); + if (m_userPairCallback) + m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); + + } + + + // update edge reference in other handle + pHandleNext->m_maxEdges[axis]--; + } + else + pHandleNext->m_minEdges[axis]--; + + pHandleEdge->m_minEdges[axis]++; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pNext; + *pNext = swap; + + // increment + pEdge++; + pNext++; + } + + +} + +// sorting a max edge downwards can only ever *remove* overlaps +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) +{ + + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pPrev = pEdge - 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos < pPrev->m_pos) + { + Handle* pHandlePrev = getHandle(pPrev->m_handle); + + if (!pPrev->IsMax()) + { + // if previous edge was a minimum remove any overlap between the two handles + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pPrev->m_handle); + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + + if (updateOverlaps +#ifdef USE_OVERLAP_TEST_ON_REMOVES + && testOverlap2D(handle0,handle1,axis1,axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) + { + //this is done during the overlappingpairarray iteration/narrowphase collision + + + m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); + if (m_userPairCallback) + m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); + + + + } + + // update edge reference in other handle + pHandlePrev->m_minEdges[axis]++;; + } + else + pHandlePrev->m_maxEdges[axis]++; + + pHandleEdge->m_maxEdges[axis]--; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pPrev; + *pPrev = swap; + + // decrement + pEdge--; + pPrev--; + } + + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + +} + +// sorting a max edge upwards can only ever *add* overlaps +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pNext = pEdge + 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) + { + Handle* pHandleNext = getHandle(pNext->m_handle); + + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + + if (!pNext->IsMax()) + { + // if next edge is a minimum check the bounds and add an overlap if necessary + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2)) + { + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pNext->m_handle); + m_pairCache->addOverlappingPair(handle0,handle1); + if (m_userPairCallback) + m_userPairCallback->addOverlappingPair(handle0,handle1); + } + + // update edge reference in other handle + pHandleNext->m_minEdges[axis]--; + } + else + pHandleNext->m_maxEdges[axis]--; + + pHandleEdge->m_maxEdges[axis]++; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pNext; + *pNext = swap; + + // increment + pEdge++; + pNext++; + } + +} + + + +//////////////////////////////////////////////////////////////////// + + +/// The 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 16 bit integer coordinates instead of floats. +/// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance. +class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int> +{ +public: + + btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0); + +}; + +/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune. +/// This comes at the cost of more memory per handle, and a bit slower performance. +/// It uses arrays rather then lists for storage of the 3 axis. +class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int> +{ +public: + + bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0); }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index b6ace03c07a..200ac365329 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -20,20 +20,34 @@ subject to the following restrictions: struct btDispatcherInfo; class btDispatcher; -struct btBroadphaseProxy; -#include "../../LinearMath/btVector3.h" +#include "btBroadphaseProxy.h" +class btOverlappingPairCache; -///BroadphaseInterface for aabb-overlapping object pairs +#include "LinearMath/btVector3.h" + +///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs. +///Some implementations for this broadphase interface include btAxisSweep3, bt32BitAxisSweep3 and btDbvtBroadphase. +///The actual overlapping pair management, storage, adding and removing of pairs is dealt by the btOverlappingPairCache class. class btBroadphaseInterface { public: virtual ~btBroadphaseInterface() {} - virtual btBroadphaseProxy* createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0; - virtual void destroyProxy(btBroadphaseProxy* proxy)=0; - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)=0; - virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy)=0; + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0; + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0; + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0; + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb + virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; + + virtual btOverlappingPairCache* getOverlappingPairCache()=0; + virtual const btOverlappingPairCache* getOverlappingPairCache() const =0; + + ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame + ///will add some transform later + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0; + + virtual void printStats() = 0; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index 40d9748ffa9..a074a0b150b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -16,7 +16,8 @@ subject to the following restrictions: #ifndef BROADPHASE_PROXY_H #define BROADPHASE_PROXY_H -#include "../../LinearMath/btScalar.h" //for SIMD_FORCE_INLINE +#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE +#include "LinearMath/btAlignedAllocator.h" /// btDispatcher uses these types @@ -38,18 +39,22 @@ IMPLICIT_CONVEX_SHAPES_START_HERE, CONE_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE, CYLINDER_SHAPE_PROXYTYPE, + UNIFORM_SCALING_SHAPE_PROXYTYPE, MINKOWSKI_SUM_SHAPE_PROXYTYPE, MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, //concave shapes CONCAVE_SHAPES_START_HERE, //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! TRIANGLE_MESH_SHAPE_PROXYTYPE, + SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE, ///used for demo integration FAST/Swift collision library and Bullet FAST_CONCAVE_MESH_PROXYTYPE, //terrain TERRAIN_SHAPE_PROXYTYPE, ///Used for GIMPACT Trimesh integration GIMPACT_SHAPE_PROXYTYPE, +///Multimaterial mesh + MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE, EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, @@ -57,13 +62,18 @@ CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, + SOFTBODY_SHAPE_PROXYTYPE, + MAX_BROADPHASE_COLLISION_TYPES }; -///btBroadphaseProxy -struct btBroadphaseProxy +///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. +///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody. +ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy { + +BT_DECLARE_ALIGNED_ALLOCATOR(); ///optional filtering to cull potential collisions enum CollisionFilterGroups @@ -73,44 +83,60 @@ struct btBroadphaseProxy KinematicFilter = 4, DebrisFilter = 8, SensorTrigger = 16, - AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger + AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger }; //Usually the client btCollisionObject or Rigidbody class void* m_clientObject; + short int m_collisionFilterGroup; short int m_collisionFilterMask; + void* m_multiSapParentProxy; + + + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. + + SIMD_FORCE_INLINE int getUid() const + { + return m_uniqueId; + } + //used for memory pools - btBroadphaseProxy() :m_clientObject(0){} + btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0) + { + } - btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) + btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) :m_clientObject(userPtr), m_collisionFilterGroup(collisionFilterGroup), m_collisionFilterMask(collisionFilterMask) { + m_multiSapParentProxy = multiSapParentProxy; } - static inline bool isPolyhedral(int proxyType) + + + static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType) { return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE); } - static inline bool isConvex(int proxyType) + static SIMD_FORCE_INLINE bool isConvex(int proxyType) { return (proxyType < CONCAVE_SHAPES_START_HERE); } - static inline bool isConcave(int proxyType) + static SIMD_FORCE_INLINE bool isConcave(int proxyType) { return ((proxyType > CONCAVE_SHAPES_START_HERE) && (proxyType < CONCAVE_SHAPES_END_HERE)); } - static inline bool isCompound(int proxyType) + static SIMD_FORCE_INLINE bool isCompound(int proxyType) { return (proxyType == COMPOUND_SHAPE_PROXYTYPE); } - static inline bool isInfinite(int proxyType) + static SIMD_FORCE_INLINE bool isInfinite(int proxyType) { return (proxyType == STATIC_PLANE_PROXYTYPE); } @@ -124,8 +150,9 @@ struct btBroadphaseProxy; -/// contains a pair of aabb-overlapping objects -struct btBroadphasePair +///The btBroadphasePair class contains a pair of aabb-overlapping objects. +///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes. +ATTRIBUTE_ALIGNED16(struct) btBroadphasePair { btBroadphasePair () : @@ -136,6 +163,8 @@ struct btBroadphasePair { } +BT_DECLARE_ALIGNED_ALLOCATOR(); + btBroadphasePair(const btBroadphasePair& other) : m_pProxy0(other.m_pProxy0), m_pProxy1(other.m_pProxy1), @@ -181,6 +210,7 @@ SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePa */ + class btBroadphasePairSortPredicate { public: diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp index 2ad0c86d8a2..c95d1be0f2c 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp @@ -18,6 +18,6 @@ subject to the following restrictions: btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) { - m_dispatcher = ci.m_dispatcher; + m_dispatcher = ci.m_dispatcher1; } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index 55cec386a7b..1618ad9fdd3 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -16,7 +16,8 @@ subject to the following restrictions: #ifndef COLLISION_ALGORITHM_H #define COLLISION_ALGORITHM_H -#include "../../LinearMath/btScalar.h" +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" struct btBroadphaseProxy; class btDispatcher; @@ -25,21 +26,22 @@ class btCollisionObject; struct btDispatcherInfo; class btPersistentManifold; +typedef btAlignedObjectArray<btPersistentManifold*> btManifoldArray; struct btCollisionAlgorithmConstructionInfo { btCollisionAlgorithmConstructionInfo() - :m_dispatcher(0), + :m_dispatcher1(0), m_manifold(0) { } btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) - :m_dispatcher(dispatcher) + :m_dispatcher1(dispatcher) { (void)temp; } - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher1; btPersistentManifold* m_manifold; int getDispatcherId(); @@ -71,6 +73,7 @@ public: virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp new file mode 100644 index 00000000000..7c41c8d8f71 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -0,0 +1,1289 @@ +/* +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. +*/ +///btDbvt implementation by Nathanael Presson + +#include "btDbvt.h" + +// +typedef btAlignedObjectArray<btDbvtNode*> tNodeArray; +typedef btAlignedObjectArray<const btDbvtNode*> tConstNodeArray; + +// +struct btDbvtNodeEnumerator : btDbvt::ICollide +{ +tConstNodeArray nodes; +void Process(const btDbvtNode* n) { nodes.push_back(n); } +}; + +// +static DBVT_INLINE int indexof(const btDbvtNode* node) +{ +return(node->parent->childs[1]==node); +} + +// +static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, + const btDbvtVolume& b) +{ +#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE +DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)]; +btDbvtVolume& res=*(btDbvtVolume*)locals; +#else +btDbvtVolume res; +#endif +Merge(a,b,res); +return(res); +} + +// volume+edge lengths +static DBVT_INLINE btScalar size(const btDbvtVolume& a) +{ +const btVector3 edges=a.Lengths(); +return( edges.x()*edges.y()*edges.z()+ + edges.x()+edges.y()+edges.z()); +} + +// +static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) +{ +if(node->isinternal()) + { + getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[0],depth+1,maxdepth); + } else maxdepth=btMax(maxdepth,depth); +} + +// +static DBVT_INLINE void deletenode( btDbvt* pdbvt, + btDbvtNode* node) +{ +btAlignedFree(pdbvt->m_free); +pdbvt->m_free=node; +} + +// +static void recursedeletenode( btDbvt* pdbvt, + btDbvtNode* node) +{ +if(!node->isleaf()) + { + recursedeletenode(pdbvt,node->childs[0]); + recursedeletenode(pdbvt,node->childs[1]); + } +if(node==pdbvt->m_root) pdbvt->m_root=0; +deletenode(pdbvt,node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + void* data) +{ +btDbvtNode* node; +if(pdbvt->m_free) + { node=pdbvt->m_free;pdbvt->m_free=0; } + else + { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } +node->parent = parent; +node->data = data; +node->childs[1] = 0; +return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume, + void* data) +{ +btDbvtNode* node=createnode(pdbvt,parent,data); +node->volume=volume; +return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume0, + const btDbvtVolume& volume1, + void* data) +{ +btDbvtNode* node=createnode(pdbvt,parent,data); +Merge(volume0,volume1,node->volume); +return(node); +} + +// +static void insertleaf( btDbvt* pdbvt, + btDbvtNode* root, + btDbvtNode* leaf) +{ +if(!pdbvt->m_root) + { + pdbvt->m_root = leaf; + leaf->parent = 0; + } + else + { + if(!root->isleaf()) + { + do { + root=root->childs[Select( leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; + } while(!root->isleaf()); + } + btDbvtNode* prev=root->parent; + btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); + if(prev) + { + prev->childs[indexof(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + else + break; + node=prev; + } while(0!=(prev=node->parent)); + } + else + { + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + pdbvt->m_root = node; + } + } +} + +// +static btDbvtNode* removeleaf( btDbvt* pdbvt, + btDbvtNode* leaf) +{ +if(leaf==pdbvt->m_root) + { + pdbvt->m_root=0; + return(0); + } + else + { + btDbvtNode* parent=leaf->parent; + btDbvtNode* prev=parent->parent; + btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; + if(prev) + { + prev->childs[indexof(parent)]=sibling; + sibling->parent=prev; + deletenode(pdbvt,parent); + while(prev) + { + const btDbvtVolume pb=prev->volume; + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(NotEqual(pb,prev->volume)) + { + prev=prev->parent; + } else break; + } + return(prev?prev:pdbvt->m_root); + } + else + { + pdbvt->m_root=sibling; + sibling->parent=0; + deletenode(pdbvt,parent); + return(pdbvt->m_root); + } + } +} + +// +static void fetchleaves(btDbvt* pdbvt, + btDbvtNode* root, + tNodeArray& leaves, + int depth=-1) +{ +if(root->isinternal()&&depth) + { + fetchleaves(pdbvt,root->childs[0],leaves,depth-1); + fetchleaves(pdbvt,root->childs[1],leaves,depth-1); + deletenode(pdbvt,root); + } + else + { + leaves.push_back(root); + } +} + +// +static void split( const tNodeArray& leaves, + tNodeArray& left, + tNodeArray& right, + const btVector3& org, + const btVector3& axis) +{ +left.resize(0); +right.resize(0); +for(int i=0,ni=leaves.size();i<ni;++i) + { + if(dot(axis,leaves[i]->volume.Center()-org)<0) + left.push_back(leaves[i]); + else + right.push_back(leaves[i]); + } +} + +// +static btDbvtVolume bounds( const tNodeArray& leaves) +{ +#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE +DBVT_ALIGN char locals[sizeof(btDbvtVolume)]; +btDbvtVolume& volume=*(btDbvtVolume*)locals; +volume=leaves[0]->volume; +#else +btDbvtVolume volume=leaves[0]->volume; +#endif +for(int i=1,ni=leaves.size();i<ni;++i) + { + Merge(volume,leaves[i]->volume,volume); + } +return(volume); +} + +// +static void bottomup( btDbvt* pdbvt, + tNodeArray& leaves) +{ +while(leaves.size()>1) + { + btScalar minsize=SIMD_INFINITY; + int minidx[2]={-1,-1}; + for(int i=0;i<leaves.size();++i) + { + for(int j=i+1;j<leaves.size();++j) + { + const btScalar sz=size(merge(leaves[i]->volume,leaves[j]->volume)); + if(sz<minsize) + { + minsize = sz; + minidx[0] = i; + minidx[1] = j; + } + } + } + btDbvtNode* n[] = {leaves[minidx[0]],leaves[minidx[1]]}; + btDbvtNode* p = createnode(pdbvt,0,n[0]->volume,n[1]->volume,0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves.swap(minidx[1],leaves.size()-1); + leaves.pop_back(); + } +} + +// +static btDbvtNode* topdown(btDbvt* pdbvt, + tNodeArray& leaves, + int bu_treshold) +{ +static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; +if(leaves.size()>1) + { + if(leaves.size()>bu_treshold) + { + const btDbvtVolume vol=bounds(leaves); + const btVector3 org=vol.Center(); + tNodeArray sets[2]; + int bestaxis=-1; + int bestmidp=leaves.size(); + int splitcount[3][2]={{0,0},{0,0},{0,0}}; + int i; + for( i=0;i<leaves.size();++i) + { + const btVector3 x=leaves[i]->volume.Center()-org; + for(int j=0;j<3;++j) + { + ++splitcount[j][dot(x,axis[j])>0?1:0]; + } + } + for( i=0;i<3;++i) + { + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + { + const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); + if(midp<bestmidp) + { + bestaxis=i; + bestmidp=midp; + } + } + } + if(bestaxis>=0) + { + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); + split(leaves,sets[0],sets[1],org,axis[bestaxis]); + } + else + { + sets[0].reserve(leaves.size()/2+1); + sets[1].reserve(leaves.size()/2); + for(int i=0,ni=leaves.size();i<ni;++i) + { + sets[i&1].push_back(leaves[i]); + } + } + btDbvtNode* node=createnode(pdbvt,0,vol,0); + node->childs[0]=topdown(pdbvt,sets[0],bu_treshold); + node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); + } + else + { + bottomup(pdbvt,leaves); + return(leaves[0]); + } + } +return(leaves[0]); +} + +// +static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) +{ +btDbvtNode* p=n->parent; +btAssert(n->isinternal()); +if(p>n) + { + const int i=indexof(n); + const int j=1-i; + btDbvtNode* s=p->childs[j]; + btDbvtNode* q=p->parent; + btAssert(n==p->childs[i]); + if(q) q->childs[indexof(p)]=n; else r=n; + s->parent=n; + p->parent=n; + n->parent=q; + p->childs[0]=n->childs[0]; + p->childs[1]=n->childs[1]; + n->childs[0]->parent=p; + n->childs[1]->parent=p; + n->childs[i]=p; + n->childs[j]=s; + btSwap(p->volume,n->volume); + return(p); + } +return(n); +} + +// +static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) +{ +while(n&&(count--)) n=n->parent; +return(n); +} + +// +// Api +// + +// + btDbvt::btDbvt() +{ +m_root = 0; +m_free = 0; +m_lkhd = -1; +m_leaves = 0; +m_opath = 0; +} + +// + btDbvt::~btDbvt() +{ +clear(); +} + +// +void btDbvt::clear() +{ +if(m_root) recursedeletenode(this,m_root); +btAlignedFree(m_free); +m_free=0; +} + +// +void btDbvt::optimizeBottomUp() +{ +if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + bottomup(this,leaves); + m_root=leaves[0]; + } +} + +// +void btDbvt::optimizeTopDown(int bu_treshold) +{ +if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + m_root=topdown(this,leaves,bu_treshold); + } +} + +// +void btDbvt::optimizeIncremental(int passes) +{ +if(passes<0) passes=m_leaves; +if(m_root&&(passes>0)) + { + do { + btDbvtNode* node=m_root; + unsigned bit=0; + while(node->isinternal()) + { + node=sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); + } + update(node); + ++m_opath; + } while(--passes); + } +} + +// +btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) +{ +btDbvtNode* leaf=createnode(this,0,volume,data); +insertleaf(this,m_root,leaf); +++m_leaves; +return(leaf); +} + +// +void btDbvt::update(btDbvtNode* leaf,int lookahead) +{ +btDbvtNode* root=removeleaf(this,leaf); +if(root) + { + if(lookahead>=0) + { + for(int i=0;(i<lookahead)&&root->parent;++i) + { + root=root->parent; + } + } else root=m_root; + } +insertleaf(this,root,leaf); +} + +// +void btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume) +{ +btDbvtNode* root=removeleaf(this,leaf); +if(root) + { + if(m_lkhd>=0) + { + for(int i=0;(i<m_lkhd)&&root->parent;++i) + { + root=root->parent; + } + } else root=m_root; + } +leaf->volume=volume; +insertleaf(this,root,leaf); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +update(leaf,volume); +return(true); +} + +// +void btDbvt::remove(btDbvtNode* leaf) +{ +removeleaf(this,leaf); +deletenode(this,leaf); +--m_leaves; +} + +// +void btDbvt::write(IWriter* iwriter) const +{ +btDbvtNodeEnumerator nodes; +nodes.nodes.reserve(m_leaves*2); +enumNodes(m_root,nodes); +iwriter->Prepare(m_root,nodes.nodes.size()); +for(int i=0;i<nodes.nodes.size();++i) + { + const btDbvtNode* n=nodes.nodes[i]; + int p=-1; + if(n->parent) p=nodes.nodes.findLinearSearch(n->parent); + if(n->isinternal()) + { + const int c0=nodes.nodes.findLinearSearch(n->childs[0]); + const int c1=nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n,i,p,c0,c1); + } + else + { + iwriter->WriteLeaf(n,i,p); + } + } +} + +// +void btDbvt::clone(btDbvt& dest,IClone* iclone) const +{ +dest.clear(); +if(m_root!=0) + { + btAlignedObjectArray<sStkCLN> stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); + stack.pop_back(); + if(e.parent!=0) + e.parent->childs[i&1]=n; + else + dest.m_root=n; + if(e.node->isinternal()) + { + stack.push_back(sStkCLN(e.node->childs[0],n)); + stack.push_back(sStkCLN(e.node->childs[1],n)); + } + else + { + iclone->CloneLeaf(n); + } + } while(stack.size()>0); + } +} + +// +int btDbvt::maxdepth(const btDbvtNode* node) +{ +int depth=0; +if(node) getmaxdepth(node,1,depth); +return(depth); +} + +// +int btDbvt::countLeaves(const btDbvtNode* node) +{ +if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + else + return(1); +} + +// +void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves) +{ +if(node->isinternal()) + { + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); + } + else + { + leaves.push_back(node); + } +} + +// +#if DBVT_ENABLE_BENCHMARK + +#include <stdio.h> +#include <stdlib.h> +#include "LinearMath/btQuickProf.h" + +/* +q6600,2.4ghz + +/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32" +/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch" +/Fo"..\..\out\release8\build\libbulletcollision\\" +/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb" +/W3 /nologo /c /Wp64 /Zi /errorReport:prompt + +Benchmarking dbvt... + World scale: 100.000000 + Extents base: 1.000000 + Extents range: 4.000000 + Leaves: 8192 + sizeof(btDbvtVolume): 32 bytes + sizeof(btDbvtNode): 44 bytes +[1] btDbvtVolume intersections: 3499 ms (-1%) +[2] btDbvtVolume merges: 1934 ms (0%) +[3] btDbvt::collideTT: 5485 ms (-21%) +[4] btDbvt::collideTT self: 2814 ms (-20%) +[5] btDbvt::collideTT xform: 7379 ms (-1%) +[6] btDbvt::collideTT xform,self: 7270 ms (-2%) +[7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s) +[8] insert/remove: 2093 ms (0%),(1001983 ir/s) +[9] updates (teleport): 1879 ms (-3%),(1116100 u/s) +[10] updates (jitter): 1244 ms (-4%),(1685813 u/s) +[11] optimize (incremental): 2514 ms (0%),(1668000 o/s) +[12] btDbvtVolume notequal: 3659 ms (0%) +[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) +[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) +[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) +[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) +[17] btDbvtVolume select: 3419 ms (0%) +*/ + +struct btDbvtBenchmark +{ +struct NilPolicy : btDbvt::ICollide + { + NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} + void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*,btScalar depth) + { + ++m_pcount; + if(m_checksort) + { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } + } + int m_pcount; + btScalar m_depth; + bool m_checksort; + }; +struct P14 : btDbvt::ICollide + { + struct Node + { + const btDbvtNode* leaf; + btScalar depth; + }; + void Process(const btDbvtNode* leaf,btScalar depth) + { + Node n; + n.leaf = leaf; + n.depth = depth; + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depth<b.depth) return(+1); + if(a.depth>b.depth) return(-1); + return(0); + } + btAlignedObjectArray<Node> m_nodes; + }; +struct P15 : btDbvt::ICollide + { + struct Node + { + const btDbvtNode* leaf; + btScalar depth; + }; + void Process(const btDbvtNode* leaf) + { + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(),m_axis); + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depth<b.depth) return(+1); + if(a.depth>b.depth) return(-1); + return(0); + } + btAlignedObjectArray<Node> m_nodes; + btVector3 m_axis; + }; +static btScalar RandUnit() + { + return(rand()/(btScalar)RAND_MAX); + } +static btVector3 RandVector3() + { + return(btVector3(RandUnit(),RandUnit(),RandUnit())); + } +static btVector3 RandVector3(btScalar cs) + { + return(RandVector3()*cs-btVector3(cs,cs,cs)/2); + } +static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) + { + return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); + } +static btTransform RandTransform(btScalar cs) + { + btTransform t; + t.setOrigin(RandVector3(cs)); + t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); + return(t); + } +static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) + { + dbvt.clear(); + for(int i=0;i<leaves;++i) + { + dbvt.insert(RandVolume(cs,eb,es),0); + } + } +}; + +void btDbvt::benchmark() +{ +static const btScalar cfgVolumeCenterScale = 100; +static const btScalar cfgVolumeExentsBase = 1; +static const btScalar cfgVolumeExentsScale = 4; +static const int cfgLeaves = 8192; +static const bool cfgEnable = true; + +//[1] btDbvtVolume intersections +bool cfgBenchmark1_Enable = cfgEnable; +static const int cfgBenchmark1_Iterations = 8; +static const int cfgBenchmark1_Reference = 3499; +//[2] btDbvtVolume merges +bool cfgBenchmark2_Enable = cfgEnable; +static const int cfgBenchmark2_Iterations = 4; +static const int cfgBenchmark2_Reference = 1945; +//[3] btDbvt::collideTT +bool cfgBenchmark3_Enable = cfgEnable; +static const int cfgBenchmark3_Iterations = 512; +static const int cfgBenchmark3_Reference = 5485; +//[4] btDbvt::collideTT self +bool cfgBenchmark4_Enable = cfgEnable; +static const int cfgBenchmark4_Iterations = 512; +static const int cfgBenchmark4_Reference = 2814; +//[5] btDbvt::collideTT xform +bool cfgBenchmark5_Enable = cfgEnable; +static const int cfgBenchmark5_Iterations = 512; +static const btScalar cfgBenchmark5_OffsetScale = 2; +static const int cfgBenchmark5_Reference = 7379; +//[6] btDbvt::collideTT xform,self +bool cfgBenchmark6_Enable = cfgEnable; +static const int cfgBenchmark6_Iterations = 512; +static const btScalar cfgBenchmark6_OffsetScale = 2; +static const int cfgBenchmark6_Reference = 7270; +//[7] btDbvt::collideRAY +bool cfgBenchmark7_Enable = cfgEnable; +static const int cfgBenchmark7_Passes = 32; +static const int cfgBenchmark7_Iterations = 65536; +static const int cfgBenchmark7_Reference = 6307; +//[8] insert/remove +bool cfgBenchmark8_Enable = cfgEnable; +static const int cfgBenchmark8_Passes = 32; +static const int cfgBenchmark8_Iterations = 65536; +static const int cfgBenchmark8_Reference = 2105; +//[9] updates (teleport) +bool cfgBenchmark9_Enable = cfgEnable; +static const int cfgBenchmark9_Passes = 32; +static const int cfgBenchmark9_Iterations = 65536; +static const int cfgBenchmark9_Reference = 1879; +//[10] updates (jitter) +bool cfgBenchmark10_Enable = cfgEnable; +static const btScalar cfgBenchmark10_Scale = cfgVolumeCenterScale/10000; +static const int cfgBenchmark10_Passes = 32; +static const int cfgBenchmark10_Iterations = 65536; +static const int cfgBenchmark10_Reference = 1244; +//[11] optimize (incremental) +bool cfgBenchmark11_Enable = cfgEnable; +static const int cfgBenchmark11_Passes = 64; +static const int cfgBenchmark11_Iterations = 65536; +static const int cfgBenchmark11_Reference = 2510; +//[12] btDbvtVolume notequal +bool cfgBenchmark12_Enable = cfgEnable; +static const int cfgBenchmark12_Iterations = 32; +static const int cfgBenchmark12_Reference = 3677; +//[13] culling(OCL+fullsort) +bool cfgBenchmark13_Enable = cfgEnable; +static const int cfgBenchmark13_Iterations = 1024; +static const int cfgBenchmark13_Reference = 2231; +//[14] culling(OCL+qsort) +bool cfgBenchmark14_Enable = cfgEnable; +static const int cfgBenchmark14_Iterations = 8192; +static const int cfgBenchmark14_Reference = 3500; +//[15] culling(KDOP+qsort) +bool cfgBenchmark15_Enable = cfgEnable; +static const int cfgBenchmark15_Iterations = 8192; +static const int cfgBenchmark15_Reference = 1151; +//[16] insert/remove batch +bool cfgBenchmark16_Enable = cfgEnable; +static const int cfgBenchmark16_BatchCount = 256; +static const int cfgBenchmark16_Passes = 16384; +static const int cfgBenchmark16_Reference = 5138; +//[17] select +bool cfgBenchmark17_Enable = cfgEnable; +static const int cfgBenchmark17_Iterations = 4; +static const int cfgBenchmark17_Reference = 3390; + +btClock wallclock; +printf("Benchmarking dbvt...\r\n"); +printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale); +printf("\tExtents base: %f\r\n",cfgVolumeExentsBase); +printf("\tExtents range: %f\r\n",cfgVolumeExentsScale); +printf("\tLeaves: %u\r\n",cfgLeaves); +printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume)); +printf("\tsizeof(btDbvtNode): %u bytes\r\n",sizeof(btDbvtNode)); +if(cfgBenchmark1_Enable) + {// Benchmark 1 + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<bool> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + printf("[1] btDbvtVolume intersections: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark1_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + results[k]=Intersect(volumes[j],volumes[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time); + } +if(cfgBenchmark2_Enable) + {// Benchmark 2 + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<btDbvtVolume> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + printf("[2] btDbvtVolume merges: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark2_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + Merge(volumes[j],volumes[k],results[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time); + } +if(cfgBenchmark3_Enable) + {// Benchmark 3 + srand(380843); + btDbvt dbvt[2]; + btDbvtBenchmark::NilPolicy policy; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[3] btDbvt::collideTT: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark3_Iterations;++i) + { + btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time); + } +if(cfgBenchmark4_Enable) + {// Benchmark 4 + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::NilPolicy policy; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[4] btDbvt::collideTT self: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark4_Iterations;++i) + { + btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time); + } +if(cfgBenchmark5_Enable) + {// Benchmark 5 + srand(380843); + btDbvt dbvt[2]; + btAlignedObjectArray<btTransform> transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i<transforms.size();++i) + { + transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale); + } + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[5] btDbvt::collideTT xform: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark5_Iterations;++i) + { + btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time); + } +if(cfgBenchmark6_Enable) + {// Benchmark 6 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btTransform> transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i<transforms.size();++i) + { + transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale); + } + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[6] btDbvt::collideTT xform,self: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark6_Iterations;++i) + { + btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time); + } +if(cfgBenchmark7_Enable) + {// Benchmark 7 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> rayorg; + btAlignedObjectArray<btVector3> raydir; + btDbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i<rayorg.size();++i) + { + rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); + raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); + } + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[7] btDbvt::collideRAY: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark7_Passes;++i) + { + for(int j=0;j<cfgBenchmark7_Iterations;++j) + { + btDbvt::collideRAY(dbvt.m_root,rayorg[j],raydir[j],policy); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + unsigned rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations; + printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time); + } +if(cfgBenchmark8_Enable) + {// Benchmark 8 + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[8] insert/remove: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark8_Passes;++i) + { + for(int j=0;j<cfgBenchmark8_Iterations;++j) + { + dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0)); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations; + printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time); + } +if(cfgBenchmark9_Enable) + {// Benchmark 9 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<const btDbvtNode*> leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[9] updates (teleport): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark9_Passes;++i) + { + for(int j=0;j<cfgBenchmark9_Iterations;++j) + { + dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]), + btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); + } +if(cfgBenchmark10_Enable) + {// Benchmark 10 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<const btDbvtNode*> leaves; + btAlignedObjectArray<btVector3> vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale; + } + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[10] updates (jitter): "); + wallclock.reset(); + + for(int i=0;i<cfgBenchmark10_Passes;++i) + { + for(int j=0;j<cfgBenchmark10_Iterations;++j) + { + const btVector3& d=vectors[j]; + btDbvtNode* l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]); + btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); + dbvt.update(l,v); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); + } +if(cfgBenchmark11_Enable) + {// Benchmark 11 + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[11] optimize (incremental): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark11_Passes;++i) + { + dbvt.optimizeIncremental(cfgBenchmark11_Iterations); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations; + printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000); + } +if(cfgBenchmark12_Enable) + {// Benchmark 12 + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<bool> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + printf("[12] btDbvtVolume notequal: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark12_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + results[k]=NotEqual(volumes[j],volumes[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time); + } +if(cfgBenchmark13_Enable) + {// Benchmark 13 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + } + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[13] culling(OCL+fullsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark13_Iterations;++i) + { + static const btScalar offset=0; + policy.m_depth=-SIMD_INFINITY; + dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int t=cfgBenchmark13_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time); + } +if(cfgBenchmark14_Enable) + {// Benchmark 14 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + } + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[14] culling(OCL+qsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark14_Iterations;++i) + { + static const btScalar offset=0; + policy.m_nodes.resize(0); + dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false); + policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int t=cfgBenchmark14_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time); + } +if(cfgBenchmark15_Enable) + {// Benchmark 15 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + } + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[15] culling(KDOP+qsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark15_Iterations;++i) + { + static const btScalar offset=0; + policy.m_nodes.resize(0); + policy.m_axis=vectors[i]; + dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy); + policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int t=cfgBenchmark15_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time); + } +if(cfgBenchmark16_Enable) + {// Benchmark 16 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btDbvtNode*> batch; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + batch.reserve(cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + wallclock.reset(); + for(int i=0;i<cfgBenchmark16_Passes;++i) + { + for(int j=0;j<cfgBenchmark16_BatchCount;++j) + { + batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0)); + } + for(int j=0;j<cfgBenchmark16_BatchCount;++j) + { + dbvt.remove(batch[j]); + } + batch.resize(0); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount; + printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time)); + } +if(cfgBenchmark17_Enable) + {// Benchmark 17 + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<int> results; + btAlignedObjectArray<int> indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + indices[i]=i; + volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + for(int i=0;i<cfgLeaves;++i) + { + btSwap(indices[i],indices[rand()%cfgLeaves]); + } + printf("[17] btDbvtVolume select: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark17_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + const int idx=indices[k]; + results[idx]=Select(volumes[idx],volumes[j],volumes[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time); + } +printf("\r\n\r\n"); +} +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h new file mode 100644 index 00000000000..21d69acf151 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -0,0 +1,1112 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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. +*/ +///btDbvt implementation by Nathanael Presson + +#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H +#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H + +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" + +// +// Compile time configuration +// + + +// Implementation profiles +#define DBVT_IMPL_GENERIC 0 // Generic implementation +#define DBVT_IMPL_SSE 1 // SSE + +// Template implementation of ICollide +#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds + #if (defined (_MSC_VER) && _MSC_VER >= 1400) + #define DBVT_USE_TEMPLATE 1 + #else + #define DBVT_USE_TEMPLATE 0 +#endif +#else +#define DBVT_USE_TEMPLATE 0 +#endif + +// Use only intrinsics instead of inline asm +#define DBVT_USE_INTRINSIC_SSE 1 + +// Using memmov for collideOCL +#define DBVT_USE_MEMMOVE 1 + +// Enable benchmarking code +#define DBVT_ENABLE_BENCHMARK 0 + +// Inlining +#define DBVT_INLINE SIMD_FORCE_INLINE +// Align +#ifdef WIN32 +#define DBVT_ALIGN __declspec(align(16)) +#else +#define DBVT_ALIGN +#endif + +// Specific methods implementation + +#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds +#define DBVT_SELECT_IMPL DBVT_IMPL_SSE +#define DBVT_MERGE_IMPL DBVT_IMPL_SSE +#define DBVT_INT0_IMPL DBVT_IMPL_SSE +#else +#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC +#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC +#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC +#endif + +#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_INT0_IMPL==DBVT_IMPL_SSE) +#include <emmintrin.h> +#endif + +// +// Auto config and checks +// + +#if DBVT_USE_TEMPLATE +#define DBVT_VIRTUAL +#define DBVT_VIRTUAL_DTOR(a) +#define DBVT_PREFIX template <typename T> +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0; +#else +#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} +#define DBVT_VIRTUAL virtual +#define DBVT_PREFIX +#define DBVT_IPOLICY ICollide& policy +#define DBVT_CHECKTYPE +#endif + +#if DBVT_USE_MEMMOVE +#ifndef __CELLOS_LV2__ +#include <memory.h> +#endif +#include <string.h> +#endif + +#ifndef DBVT_USE_TEMPLATE +#error "DBVT_USE_TEMPLATE undefined" +#endif + +#ifndef DBVT_USE_MEMMOVE +#error "DBVT_USE_MEMMOVE undefined" +#endif + +#ifndef DBVT_ENABLE_BENCHMARK +#error "DBVT_ENABLE_BENCHMARK undefined" +#endif + +#ifndef DBVT_SELECT_IMPL +#error "DBVT_SELECT_IMPL undefined" +#endif + +#ifndef DBVT_MERGE_IMPL +#error "DBVT_MERGE_IMPL undefined" +#endif + +#ifndef DBVT_INT0_IMPL +#error "DBVT_INT0_IMPL undefined" +#endif + +// +// Defaults volumes +// + +/* btDbvtAabbMm */ +struct btDbvtAabbMm +{ +DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } +DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } +DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } +DBVT_INLINE const btVector3& Mins() const { return(mi); } +DBVT_INLINE const btVector3& Maxs() const { return(mx); } +static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); +static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); +static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); +static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); +static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); +DBVT_INLINE void Expand(const btVector3& e); +DBVT_INLINE void SignedExpand(const btVector3& e); +DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; +DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; +DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + const btTransform& xform); +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& b); +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& org, + const btVector3& invdir, + const unsigned* signs); +DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +DBVT_INLINE friend int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); +DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +private: +DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; +private: +btVector3 mi,mx; +}; + +// Types +typedef btDbvtAabbMm btDbvtVolume; + +/* btDbvtNode */ +struct btDbvtNode +{ + btDbvtVolume volume; + btDbvtNode* parent; + DBVT_INLINE bool isleaf() const { return(childs[1]==0); } + DBVT_INLINE bool isinternal() const { return(!isleaf()); } + union { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; + }; +}; + +///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). +///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. +///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. +struct btDbvt + { + /* Stack element */ + struct sStkNN + { + const btDbvtNode* a; + const btDbvtNode* b; + sStkNN() {} + sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} + }; + struct sStkNP + { + const btDbvtNode* node; + int mask; + sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} + }; + struct sStkNPS + { + const btDbvtNode* node; + int mask; + btScalar value; + sStkNPS() {} + sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} + }; + struct sStkCLN + { + const btDbvtNode* node; + btDbvtNode* parent; + sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} + }; + // Policies/Interfaces + + /* ICollide */ + struct ICollide + { + DBVT_VIRTUAL_DTOR(ICollide) + DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } + DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } + DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } + }; + /* IWriter */ + struct IWriter + { + virtual ~IWriter() {} + virtual void Prepare(const btDbvtNode* root,int numnodes)=0; + virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; + virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; + }; + /* IClone */ + struct IClone + { + virtual ~IClone() {} + virtual void CloneLeaf(btDbvtNode*) {} + }; + + // Constants + enum { + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + }; + + // Fields + btDbvtNode* m_root; + btDbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + // Methods + btDbvt(); + ~btDbvt(); + void clear(); + bool empty() const { return(0==m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold=128); + void optimizeIncremental(int passes); + btDbvtNode* insert(const btDbvtVolume& box,void* data); + void update(btDbvtNode* leaf,int lookahead=-1); + void update(btDbvtNode* leaf,const btDbvtVolume& volume); + bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin); + bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity); + bool update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin); + void remove(btDbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(btDbvt& dest,IClone* iclone=0) const; + static int maxdepth(const btDbvtNode* node); + static int countLeaves(const btDbvtNode* node); + static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves); + #if DBVT_ENABLE_BENCHMARK + static void benchmark(); + #else + static void benchmark(){} + #endif + // DBVT_IPOLICY must support ICollide policy/interface + DBVT_PREFIX + static void enumNodes( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTV( const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideRAY( const btDbvtNode* root, + const btVector3& origin, + const btVector3& direction, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort=true); + DBVT_PREFIX + static void collideTU( const btDbvtNode* root, + DBVT_IPOLICY); + // Helpers + static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) + { + int m=0; + while(l<h) + { + m=(l+h)>>1; + if(a[i[m]].value>=v) l=m+1; else h=m; + } + return(h); + } + static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree, + btAlignedObjectArray<sStkNPS>& stock, + const sStkNPS& value) + { + int i; + if(ifree.size()>0) + { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + else + { i=stock.size();stock.push_back(value); } + return(i); + } + // + private: + btDbvt(const btDbvt&) {} + }; + +// +// Inline's +// + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) +{ +btDbvtAabbMm box; +box.mi=c-e;box.mx=c+e; +return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) +{ +return(FromCE(c,btVector3(r,r,r))); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) +{ +btDbvtAabbMm box; +box.mi=mi;box.mx=mx; +return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) +{ +btDbvtAabbMm box; +box.mi=box.mx=pts[0]; +for(int i=1;i<n;++i) + { + box.mi.setMin(pts[i]); + box.mx.setMax(pts[i]); + } +return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n) +{ +btDbvtAabbMm box; +box.mi=box.mx=*ppts[0]; +for(int i=1;i<n;++i) + { + box.mi.setMin(*ppts[i]); + box.mx.setMax(*ppts[i]); + } +return(box); +} + +// +DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) +{ +mi-=e;mx+=e; +} + +// +DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e) +{ +if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); +if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); +if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); +} + +// +DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const +{ +return( (mi.x()<=a.mi.x())&& + (mi.y()<=a.mi.y())&& + (mi.z()<=a.mi.z())&& + (mx.x()>=a.mx.x())&& + (mx.y()>=a.mx.y())&& + (mx.z()>=a.mx.z())); +} + +// +DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const +{ +btVector3 pi,px; +switch(s) + { + case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); + pi=btVector3(mx.x(),mx.y(),mx.z());break; + case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); + pi=btVector3(mi.x(),mx.y(),mx.z());break; + case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); + pi=btVector3(mx.x(),mi.y(),mx.z());break; + case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); + pi=btVector3(mi.x(),mi.y(),mx.z());break; + case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); + pi=btVector3(mx.x(),mx.y(),mi.z());break; + case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); + pi=btVector3(mi.x(),mx.y(),mi.z());break; + case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); + pi=btVector3(mx.x(),mi.y(),mi.z());break; + case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); + pi=btVector3(mi.x(),mi.y(),mi.z());break; + } +if((dot(n,px)+o)<0) return(-1); +if((dot(n,pi)+o)>=0) return(+1); +return(0); +} + +// +DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const +{ +const btVector3* b[]={&mx,&mi}; +const btVector3 p( b[(signs>>0)&1]->x(), + b[(signs>>1)&1]->y(), + b[(signs>>2)&1]->z()); +return(dot(p,v)); +} + +// +DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const +{ +for(int i=0;i<3;++i) + { + if(d[i]<0) + { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } + else + { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } + } +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_INT0_IMPL == DBVT_IMPL_SSE +const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); +const __int32* pu((const __int32*)&rt); +return((pu[0]|pu[1]|pu[2])==0); +#else +return( (a.mi.x()<=b.mx.x())&& + (a.mx.x()>=b.mi.x())&& + (a.mi.y()<=b.mx.y())&& + (a.mx.y()>=b.mi.y())&& + (a.mi.z()<=b.mx.z())&& + (a.mx.z()>=b.mi.z())); +#endif +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + const btTransform& xform) +{ +const btVector3 d0=xform*b.Center()-a.Center(); +const btVector3 d1=d0*xform.getBasis(); +btScalar s0[2]={0,0}; +btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]}; +a.AddSpan(d0,s0[0],s0[1]); +b.AddSpan(d1,s1[0],s1[1]); +if(s0[0]>(s1[1])) return(false); +if(s0[1]<(s1[0])) return(false); +return(true); +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btVector3& b) +{ +return( (b.x()>=a.mi.x())&& + (b.y()>=a.mi.y())&& + (b.z()>=a.mi.z())&& + (b.x()<=a.mx.x())&& + (b.y()<=a.mx.y())&& + (b.z()<=a.mx.z())); +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btVector3& org, + const btVector3& invdir, + const unsigned* signs) +{ +#if 0 +const btVector3 b0((a.mi-org)*invdir); +const btVector3 b1((a.mx-org)*invdir); +const btVector3 tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2])); +const btVector3 tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2])); +const btScalar tin=btMax(tmin[0],btMax(tmin[1],tmin[2])); +const btScalar tout=btMin(tmax[0],btMin(tmax[1],tmax[2])); +return(tin<tout); +#else +const btVector3* bounds[2]={&a.mi,&a.mx}; +btScalar txmin=(bounds[ signs[0]]->x()-org[0])*invdir[0]; +btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0]; +const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1]; +const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1]; +if((txmin>tymax)||(tymin>txmax)) return(false); +if(tymin>txmin) txmin=tymin; +if(tymax<txmax) txmax=tymax; +const btScalar tzmin=(bounds[ signs[2]]->z()-org[2])*invdir[2]; +const btScalar tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2]; +if((txmin>tzmax)||(tzmin>txmax)) return(false); +if(tzmin>txmin) txmin=tzmin; +if(tzmax<txmax) txmax=tzmax; +return(txmax>0); +#endif +} + +// +DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); +return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); +} + +// +DBVT_INLINE int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE +static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; + // TODO: the intrinsic version is 11% slower + #if DBVT_USE_INTRINSIC_SSE + __m128 omi(_mm_load_ps(o.mi)); + omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); + __m128 ami(_mm_load_ps(a.mi)); + ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); + ami=_mm_sub_ps(ami,omi); + ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); + __m128 bmi(_mm_load_ps(b.mi)); + bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); + bmi=_mm_sub_ps(bmi,omi); + bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); + __m128 t0(_mm_movehl_ps(ami,ami)); + ami=_mm_add_ps(ami,t0); + ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); + bmi=_mm_add_ps(bmi,t1); + bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); + return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); + #else + DBVT_ALIGN __int32 r[1]; + __asm + { + mov eax,o + mov ecx,a + mov edx,b + movaps xmm0,[eax] + movaps xmm5,mask + addps xmm0,[eax+16] + movaps xmm1,[ecx] + movaps xmm2,[edx] + addps xmm1,[ecx+16] + addps xmm2,[edx+16] + subps xmm1,xmm0 + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } + return(r[0]&1); + #endif +#else +return(Proximity(o,a)<Proximity(o,b)?0:1); +#endif +} + +// +DBVT_INLINE void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r) +{ +#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE +__m128 ami(_mm_load_ps(a.mi)); +__m128 amx(_mm_load_ps(a.mx)); +__m128 bmi(_mm_load_ps(b.mi)); +__m128 bmx(_mm_load_ps(b.mx)); +ami=_mm_min_ps(ami,bmi); +amx=_mm_max_ps(amx,bmx); +_mm_store_ps(r.mi,ami); +_mm_store_ps(r.mx,amx); +#else +for(int i=0;i<3;++i) + { + if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i]; + if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + } +#endif +} + +// +DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +return( (a.mi.x()!=b.mi.x())|| + (a.mi.y()!=b.mi.y())|| + (a.mi.z()!=b.mi.z())|| + (a.mx.x()!=b.mx.x())|| + (a.mx.y()!=b.mx.y())|| + (a.mx.z()!=b.mx.z())); +} + +// +// Inline's +// + +// +DBVT_PREFIX +inline void btDbvt::enumNodes( const btDbvtNode* root, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +policy.Process(root); +if(root->isinternal()) + { + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); + } +} + +// +DBVT_PREFIX +inline void btDbvt::enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root0&&root1) + { + btAlignedObjectArray<sStkNN> stack; + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=sStkNN(root0,root1); + do { + sStkNN p=stack[--depth]; + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stack[depth++]=sStkNN(p.a->childs[0],p.b); + stack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a,p.b->childs[0]); + stack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root0&&root1) + { + btAlignedObjectArray<sStkNN> stack; + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=sStkNN(root0,root1); + do { + sStkNN p=stack[--depth]; + if(Intersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stack[depth++]=sStkNN(p.a->childs[0],p.b); + stack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a,p.b->childs[0]); + stack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY) +{ +const btTransform xform=xform0.inverse()*xform1; +collideTT(root0,root1,xform,policy); +} + +// +DBVT_PREFIX +inline void btDbvt::collideTV( const btDbvtNode* root, + const btDbvtVolume& vol, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + btAlignedObjectArray<const btDbvtNode*> stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(n->volume,volume)) + { + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideRAY( const btDbvtNode* root, + const btVector3& origin, + const btVector3& direction, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + const btVector3 normal=direction.normalized(); + const btVector3 invdir( 1/normal.x(), + 1/normal.y(), + 1/normal.z()); + const unsigned signs[]={ direction.x()<0, + direction.y()<0, + direction.z()<0}; + btAlignedObjectArray<const btDbvtNode*> stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* node=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(node->volume,origin,invdir,signs)) + { + if(node->isinternal()) + { + stack.push_back(node->childs[0]); + stack.push_back(node->childs[1]); + } + else + { + policy.Process(node); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + const int inside=(1<<count)-1; + btAlignedObjectArray<sStkNP> stack; + int signs[sizeof(unsigned)*8]; + btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i<count;++i) + { + signs[i]= ((normals[i].x()>=0)?1:0)+ + ((normals[i].y()>=0)?2:0)+ + ((normals[i].z()>=0)?4:0); + } + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root,0)); + do { + sStkNP se=stack[stack.size()-1]; + bool out=false; + stack.pop_back(); + for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + { + if(0==(se.mask&j)) + { + const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(!out) + { + if((se.mask!=inside)&&(se.node->isinternal())) + { + stack.push_back(sStkNP(se.node->childs[0],se.mask)); + stack.push_back(sStkNP(se.node->childs[1],se.mask)); + } + else + { + if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) +{ +DBVT_CHECKTYPE +if(root) + { + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1<<count)-1; + btAlignedObjectArray<sStkNPS> stock; + btAlignedObjectArray<int> ifree; + btAlignedObjectArray<int> stack; + int signs[sizeof(unsigned)*8]; + btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i<count;++i) + { + signs[i]= ((normals[i].x()>=0)?1:0)+ + ((normals[i].y()>=0)?2:0)+ + ((normals[i].z()>=0)?4:0); + } + stock.reserve(SIMPLE_STACKSIZE); + stack.reserve(SIMPLE_STACKSIZE); + ifree.reserve(SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); + do { + const int id=stack[stack.size()-1]; + sStkNPS se=stock[id]; + stack.pop_back();ifree.push_back(id); + if(se.mask!=inside) + { + bool out=false; + for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + { + if(0==(se.mask&j)) + { + const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(out) continue; + } + if(policy.Descent(se.node)) + { + if(se.node->isinternal()) + { + const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; + sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), + sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; + const int q=nes[0].value<nes[1].value?1:0; + int j=stack.size(); + if(fsort&&(j>0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); + #if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); + #else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; + #endif + stack[j]=allocate(ifree,stock,nes[q]); + /* Insert 1 */ + j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); + stack.push_back(0); + #if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); + #else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; + #endif + stack[j]=allocate(ifree,stock,nes[1-q]); + } + else + { + stack.push_back(allocate(ifree,stock,nes[q])); + stack.push_back(allocate(ifree,stock,nes[1-q])); + } + } + else + { + policy.Process(se.node,se.value); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTU( const btDbvtNode* root, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + btAlignedObjectArray<const btDbvtNode*> stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } +} + +// +// PP Cleanup +// + +#undef DBVT_USE_MEMMOVE +#undef DBVT_USE_TEMPLATE +#undef DBVT_VIRTUAL_DTOR +#undef DBVT_VIRTUAL +#undef DBVT_PREFIX +#undef DBVT_IPOLICY +#undef DBVT_CHECKTYPE +#undef DBVT_IMPL_GENERIC +#undef DBVT_IMPL_SSE +#undef DBVT_USE_INTRINSIC_SSE +#undef DBVT_SELECT_IMPL +#undef DBVT_MERGE_IMPL +#undef DBVT_INT0_IMPL + +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp new file mode 100644 index 00000000000..e00fc6aa5e3 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -0,0 +1,548 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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. +*/ +///btDbvtBroadphase implementation by Nathanael Presson + +#include "btDbvtBroadphase.h" + +// +// Profiling +// + +#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK +#include <stdio.h> +#endif + +#if DBVT_BP_PROFILE +struct ProfileScope + { + __forceinline ProfileScope(btClock& clock,unsigned long& value) : + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } + __forceinline ~ProfileScope() + { + (*m_value)+=m_clock->getTimeMicroseconds()-m_base; + } + btClock* m_clock; + unsigned long* m_value; + unsigned long m_base; + }; +#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) +#else +#define SPC(_value_) +#endif + +// +// Helpers +// + +// +template <typename T> +static inline void listappend(T* item,T*& list) +{ +item->links[0]=0; +item->links[1]=list; +if(list) list->links[0]=item; +list=item; +} + +// +template <typename T> +static inline void listremove(T* item,T*& list) +{ +if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; +if(item->links[1]) item->links[1]->links[0]=item->links[0]; +} + +// +template <typename T> +static inline int listcount(T* root) +{ +int n=0; +while(root) { ++n;root=root->links[1]; } +return(n); +} + +// +template <typename T> +static inline void clear(T& value) +{ +static const struct ZeroDummy : T {} zerodummy; +value=zerodummy; +} + +// +// Colliders +// + +/* Tree collider */ +struct btDbvtTreeCollider : btDbvt::ICollide +{ +btDbvtBroadphase* pbp; +btDbvtProxy* proxy; + btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} +void Process(const btDbvtNode* na,const btDbvtNode* nb) + { + if(na!=nb) + { + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; + #if DBVT_BP_SORTPAIRS + if(pa>pb) btSwap(pa,pb); + #endif + pbp->m_paircache->addOverlappingPair(pa,pb); + ++pbp->m_newpairs; + } + } +void Process(const btDbvtNode* n) + { + Process(n,proxy->leaf); + } +}; + +// +// btDbvtBroadphase +// + +// +btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) +{ +m_deferedcollide = false; +m_needcleanup = true; +m_releasepaircache = (paircache!=0)?false:true; +m_prediction = 1/(btScalar)2; +m_stageCurrent = 0; +m_fixedleft = 0; +m_fupdates = 1; +m_dupdates = 0; +m_cupdates = 10; +m_newpairs = 1; +m_updates_call = 0; +m_updates_done = 0; +m_updates_ratio = 0; +m_paircache = paircache? + paircache : + new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); +m_gid = 0; +m_pid = 0; +m_cid = 0; +for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } +#if DBVT_BP_PROFILE +clear(m_profiling); +#endif +} + +// +btDbvtBroadphase::~btDbvtBroadphase() +{ +if(m_releasepaircache) +{ + m_paircache->~btOverlappingPairCache(); + btAlignedFree(m_paircache); +} +} + +// +btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + short int collisionFilterGroup, + short int collisionFilterMask, + btDispatcher* /*dispatcher*/, + void* /*multiSapProxy*/) +{ +btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( userPtr, + collisionFilterGroup, + collisionFilterMask); +proxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); +proxy->stage = m_stageCurrent; +proxy->m_uniqueId = ++m_gid; +proxy->leaf = m_sets[0].insert(proxy->aabb,proxy); +listappend(proxy,m_stageRoots[m_stageCurrent]); +if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + collider.proxy=proxy; + btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider); + } +return(proxy); +} + +// +void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, + btDispatcher* dispatcher) +{ +btDbvtProxy* proxy=(btDbvtProxy*)absproxy; +if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); + else + m_sets[0].remove(proxy->leaf); +listremove(proxy,m_stageRoots[proxy->stage]); +m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); +btAlignedFree(proxy); +m_needcleanup=true; +} + +// +void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) +{ +btDbvtProxy* proxy=(btDbvtProxy*)absproxy; +ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); +#if DBVT_BP_PREVENTFALSEUPDATE +if(NotEqual(aabb,proxy->leaf->volume)) +#endif + { + bool docollide=false; + if(proxy->stage==STAGECOUNT) + {/* fixed -> dynamic set */ + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; + } + else + {/* dynamic set */ + ++m_updates_call; + if(Intersect(proxy->leaf->volume,aabb)) + {/* Moving */ + const btVector3 delta=aabbMin-proxy->aabb.Mins(); + btVector3 velocity(aabb.Extents()*m_prediction); + if(delta[0]<0) velocity[0]=-velocity[0]; + if(delta[1]<0) velocity[1]=-velocity[1]; + if(delta[2]<0) velocity[2]=-velocity[2]; + if ( + #ifdef DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) + #else + m_sets[0].update(proxy->leaf,aabb,velocity) + #endif + ) + { + ++m_updates_done; + docollide=true; + } + } + else + {/* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + } + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->aabb = aabb; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider); + btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider); + } + } + } +} + +// +void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +{ +collide(dispatcher); +#if DBVT_BP_PROFILE +if(0==(m_pid%DBVT_BP_PROFILING_RATE)) + { + printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); + unsigned int total=m_profiling.m_total; + if(total<=0) total=1; + printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); + const unsigned long sum=m_profiling.m_ddcollide+ + m_profiling.m_fdcollide+ + m_profiling.m_cleanup; + printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); + printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); + clear(m_profiling); + m_clock.reset(); + } +#endif +} + +// +void btDbvtBroadphase::collide(btDispatcher* dispatcher) +{ +SPC(m_profiling.m_total); +/* optimize */ +m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); +if(m_fixedleft) + { + const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; + m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); + m_fixedleft=btMax<int>(0,m_fixedleft-count); + } +/* dynamic -> fixed set */ +m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; +btDbvtProxy* current=m_stageRoots[m_stageCurrent]; +if(current) + { + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); + #if DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); + #endif + m_sets[0].remove(current->leaf); + current->leaf = m_sets[1].insert(current->aabb,current); + current->stage = STAGECOUNT; + current = next; + } while(current); + m_fixedleft=m_sets[1].m_leaves; + m_needcleanup=true; + } +/* collide dynamics */ + { + btDbvtTreeCollider collider(this); + if(m_deferedcollide) + { + SPC(m_profiling.m_fdcollide); + btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); + } + if(m_deferedcollide) + { + SPC(m_profiling.m_ddcollide); + btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); + } + } +/* clean up */ +if(m_needcleanup) + { + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) + { + const int ci=pairs.size(); + int ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100)); + for(int i=0;i<ni;++i) + { + btBroadphasePair& p=pairs[(m_cid+i)%ci]; + btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; + btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; + if(!Intersect(pa->leaf->volume,pb->leaf->volume)) + { + #if DBVT_BP_SORTPAIRS + if(pa>pb) btSwap(pa,pb); + #endif + m_paircache->removeOverlappingPair(pa,pb,dispatcher); + --ni;--i; + } + } + if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + } + } +++m_pid; +m_newpairs=1; +m_needcleanup=false; +if(m_updates_call>0) + { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } + else + { m_updates_ratio=0; } +m_updates_done/=2; +m_updates_call/=2; +} + +// +void btDbvtBroadphase::optimize() +{ +m_sets[0].optimizeTopDown(); +m_sets[1].optimizeTopDown(); +} + +// +btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() +{ +return(m_paircache); +} + +// +const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const +{ +return(m_paircache); +} + +// +void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const +{ + + ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; + +if(!m_sets[0].empty()) + if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, + m_sets[1].m_root->volume,bounds); + else + bounds=m_sets[0].m_root->volume; +else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + else + bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); +aabbMin=bounds.Mins(); +aabbMax=bounds.Maxs(); +} + +// +void btDbvtBroadphase::printStats() +{} + +// +#if DBVT_BP_ENABLE_BENCHMARK + +struct btBroadphaseBenchmark + { + struct Experiment + { + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + btScalar speed; + btScalar amplitude; + }; + struct Object + { + btVector3 center; + btVector3 extents; + btBroadphaseProxy* proxy; + btScalar time; + void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) + { + time += speed; + center[0] = btCos(time*(btScalar)2.17)*amplitude+ + btSin(time)*amplitude/2; + center[1] = btCos(time*(btScalar)1.38)*amplitude+ + btSin(time)*amplitude; + center[2] = btSin(time*(btScalar)0.777)*amplitude; + pbi->setAabb(proxy,center-extents,center+extents,0); + } + }; + static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } + static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } + static void OutputTime(const char* name,btClock& c,unsigned count=0) + { + const unsigned long us=c.getTimeMicroseconds(); + const unsigned long ms=(us+500)/1000; + const btScalar sec=us/(btScalar)(1000*1000); + if(count>0) + printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); + else + printf("%s : %u us (%u ms)\r\n",name,us,ms); + } + }; + +void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) +{ +static const btBroadphaseBenchmark::Experiment experiments[]= + { + {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, + /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, + {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ + }; +static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); +btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects; +btClock wallclock; +/* Begin */ +for(int iexp=0;iexp<nexperiments;++iexp) + { + const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp]; + const int object_count=experiment.object_count; + const int update_count=(object_count*experiment.update_count)/100; + const int spawn_count=(object_count*experiment.spawn_count)/100; + const btScalar speed=experiment.speed; + const btScalar amplitude=experiment.amplitude; + printf("Experiment #%u '%s':\r\n",iexp,experiment.name); + printf("\tObjects: %u\r\n",object_count); + printf("\tUpdate: %u\r\n",update_count); + printf("\tSpawn: %u\r\n",spawn_count); + printf("\tSpeed: %f\r\n",speed); + printf("\tAmplitude: %f\r\n",amplitude); + srand(180673); + /* Create objects */ + wallclock.reset(); + objects.reserve(object_count); + for(int i=0;i<object_count;++i) + { + btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object(); + po->center[0]=btBroadphaseBenchmark::UnitRand()*50; + po->center[1]=btBroadphaseBenchmark::UnitRand()*50; + po->center[2]=btBroadphaseBenchmark::UnitRand()*50; + po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; + po->time=btBroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); + } + btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); + /* First update */ + wallclock.reset(); + for(int i=0;i<objects.size();++i) + { + objects[i]->update(speed,amplitude,pbi); + } + btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); + /* Updates */ + wallclock.reset(); + for(int i=0;i<experiment.iterations;++i) + { + for(int j=0;j<update_count;++j) + { + objects[j]->update(speed,amplitude,pbi); + } + pbi->calculateOverlappingPairs(0); + } + btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); + /* Clean up */ + wallclock.reset(); + for(int i=0;i<objects.size();++i) + { + pbi->destroyProxy(objects[i]->proxy,0); + delete objects[i]; + } + objects.resize(0); + btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + } + +} +#else +void btDbvtBroadphase::benchmark(btBroadphaseInterface*) +{} +#endif + +#if DBVT_BP_PROFILE +#undef SPC +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h new file mode 100644 index 00000000000..1f16043a7a8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -0,0 +1,116 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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. +*/ +///btDbvtBroadphase implementation by Nathanael Presson +#ifndef BT_DBVT_BROADPHASE_H +#define BT_DBVT_BROADPHASE_H + +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" + +// +// Compile time config +// + +#define DBVT_BP_PROFILE 0 +#define DBVT_BP_SORTPAIRS 1 +#define DBVT_BP_PREVENTFALSEUPDATE 0 +#define DBVT_BP_ACCURATESLEEPING 0 +#define DBVT_BP_ENABLE_BENCHMARK 0 +#define DBVT_BP_MARGIN (btScalar)0.05 + +#if DBVT_BP_PROFILE + #define DBVT_BP_PROFILING_RATE 256 + #include "LinearMath/btQuickprof.h" +#endif + +// +// btDbvtProxy +// +struct btDbvtProxy : btBroadphaseProxy +{ +/* Fields */ +btDbvtAabbMm aabb; +btDbvtNode* leaf; +btDbvtProxy* links[2]; +int stage; +/* ctor */ +btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : + btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask) + { + links[0]=links[1]=0; + } +}; + +typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray; + +///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt). +///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other. +///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. +struct btDbvtBroadphase : btBroadphaseInterface +{ +/* Config */ +enum { + DYNAMIC_SET = 0, /* Dynamic set index */ + FIXED_SET = 1, /* Fixed set index */ + STAGECOUNT = 2 /* Number of stages */ + }; +/* Fields */ +btDbvt m_sets[2]; // Dbvt sets +btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list +btOverlappingPairCache* m_paircache; // Pair cache +btScalar m_prediction; // Velocity prediction +int m_stageCurrent; // Current stage +int m_fupdates; // % of fixed updates per frame +int m_dupdates; // % of dynamic updates per frame +int m_cupdates; // % of cleanup updates per frame +int m_newpairs; // Number of pairs created +int m_fixedleft; // Fixed optimization left +unsigned m_updates_call; // Number of updates call +unsigned m_updates_done; // Number of updates done +btScalar m_updates_ratio; // m_updates_done/m_updates_call +int m_pid; // Parse id +int m_cid; // Cleanup index +int m_gid; // Gen id +bool m_releasepaircache; // Release pair cache on delete +bool m_deferedcollide; // Defere dynamic/static collision to collide call +bool m_needcleanup; // Need to run cleanup? +#if DBVT_BP_PROFILE +btClock m_clock; +struct { + unsigned long m_total; + unsigned long m_ddcollide; + unsigned long m_fdcollide; + unsigned long m_cleanup; + unsigned long m_jobcount; + } m_profiling; +#endif +/* Methods */ +btDbvtBroadphase(btOverlappingPairCache* paircache=0); +~btDbvtBroadphase(); +void collide(btDispatcher* dispatcher); +void optimize(); +/* btBroadphaseInterface Implementation */ +btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); +void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); +void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); +void calculateOverlappingPairs(btDispatcher* dispatcher); +btOverlappingPairCache* getOverlappingPairCache(); +const btOverlappingPairCache* getOverlappingPairCache() const; +void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; +void printStats(); +static void benchmark(btBroadphaseInterface*); +}; + +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 3d958cc8fef..6db71a0170e 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef _DISPATCHER_H #define _DISPATCHER_H -#include "../../LinearMath/btScalar.h" +#include "LinearMath/btScalar.h" class btCollisionAlgorithm; struct btBroadphaseProxy; @@ -43,7 +43,9 @@ struct btDispatcherInfo m_useContinuous(false), m_debugDraw(0), m_enableSatConvex(false), - m_enableSPU(false), + m_enableSPU(true), + m_useEpa(true), + m_allowedCcdPenetration(btScalar(0.04)), m_stackAllocator(0) { @@ -51,17 +53,19 @@ struct btDispatcherInfo btScalar m_timeStep; int m_stepCount; int m_dispatchFunc; - btScalar m_timeOfImpact; + mutable btScalar m_timeOfImpact; bool m_useContinuous; class btIDebugDraw* m_debugDraw; bool m_enableSatConvex; bool m_enableSPU; + bool m_useEpa; + btScalar m_allowedCcdPenetration; btStackAlloc* m_stackAllocator; }; -/// btDispatcher can be used in combination with broadphase to dispatch overlapping pairs. -/// For example for pairwise collision detection or user callbacks (game logic). +///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. +///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic). class btDispatcher { @@ -81,12 +85,18 @@ public: virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0; - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)=0; + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) =0; virtual int getNumManifolds() const = 0; virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0; + virtual btPersistentManifold** getInternalManifoldPointer() = 0; + + virtual void* allocateCollisionAlgorithm(int size) = 0; + + virtual void freeCollisionAlgorithm(void* ptr) = 0; + }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp new file mode 100644 index 00000000000..3f866ab7c5f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -0,0 +1,466 @@ +/* +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 "btMultiSapBroadphase.h" + +#include "btSimpleBroadphase.h" +#include "LinearMath/btAabbUtil2.h" +#include "btQuantizedBvh.h" + +/// btSapBroadphaseArray m_sapBroadphases; + +/// btOverlappingPairCache* m_overlappingPairs; +extern int gOverlappingPairs; + +/* +class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache +{ +public: + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); + } +}; + +*/ + +btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) +:m_overlappingPairs(pairCache), +m_optimizedAabbTree(0), +m_ownsPairCache(false), +m_invalidPair(0) +{ + if (!m_overlappingPairs) + { + m_ownsPairCache = true; + void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); + m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); + } + + struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback + { + virtual ~btMultiSapOverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const + { + btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; + btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; + + bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; + collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); + + return collides; + } + }; + + void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); + m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); + + m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); +// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); +// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); +} + +btMultiSapBroadphase::~btMultiSapBroadphase() +{ + if (m_ownsPairCache) + { + m_overlappingPairs->~btOverlappingPairCache(); + btAlignedFree(m_overlappingPairs); + } +} + + +void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) +{ + m_optimizedAabbTree = new btQuantizedBvh(); + m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); + QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); + for (int i=0;i<m_sapBroadphases.size();i++) + { + btQuantizedBvhNode node; + btVector3 aabbMin,aabbMax; + m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax); + m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); + m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + int partId = 0; + node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; + nodes.push_back(node); + } + m_optimizedAabbTree->buildInternal(); +} + +btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/) +{ + //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested + + void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16); + btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask); + m_multiSapProxies.push_back(proxy); + + ///this should deal with inserting/removal into child broadphases + setAabb(proxy,aabbMin,aabbMax,dispatcher); + return proxy; +} + +void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) +{ + ///not yet + btAssert(0); + +} + + +void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase) +{ + void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16); + btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy; + bridgeProxyRef->m_childProxy = childProxy; + bridgeProxyRef->m_childBroadphase = childBroadphase; + parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef); +} + + +bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax); +bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax) +{ +return +amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() && +amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() && +amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); +} + + + + + + +//#include <stdio.h> + +void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) +{ + btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy); + multiProxy->m_aabbMin = aabbMin; + multiProxy->m_aabbMax = aabbMax; + + +// bool fullyContained = false; +// bool alreadyInSimple = false; + + + + + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btMultiSapBroadphase* m_multiSap; + btMultiSapProxy* m_multiProxy; + btDispatcher* m_dispatcher; + + MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) + :m_multiSap(multiSap), + m_multiProxy(multiProxy), + m_dispatcher(dispatcher) + { + + } + + virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) + { + btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; + + int containingBroadphaseIndex = -1; + //already found? + for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++) + { + + if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) + { + containingBroadphaseIndex = i; + break; + } + } + if (containingBroadphaseIndex<0) + { + //add it + btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy); + m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase); + + } + } + }; + + MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); + + + + + m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + int i; + + for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) + { + btVector3 worldAabbMin,worldAabbMax; + multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); + bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + if (!overlapsBroadphase) + { + //remove it now + btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; + + btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; + bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); + + multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); + multiProxy->m_bridgeProxies.pop_back(); + + } + } + + + /* + + if (1) + { + + //find broadphase that contain this multiProxy + int numChildBroadphases = getBroadphaseArray().size(); + for (int i=0;i<numChildBroadphases;i++) + { + btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; + btVector3 worldAabbMin,worldAabbMax; + childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); + bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + + // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + int containingBroadphaseIndex = -1; + + //if already contains this + + for (int i=0;i<multiProxy->m_bridgeProxies.size();i++) + { + if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) + { + containingBroadphaseIndex = i; + } + alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); + } + + if (overlapsBroadphase) + { + if (containingBroadphaseIndex<0) + { + btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,childBroadphase); + } + } else + { + if (containingBroadphaseIndex>=0) + { + //remove + btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; + + btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; + bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); + + multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); + multiProxy->m_bridgeProxies.pop_back(); + } + } + } + + + ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) + ///hopefully we don't end up with many entries here (can assert/provide feedback on stats) + if (0)//!multiProxy->m_bridgeProxies.size()) + { + ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision + ///this is needed to be able to calculate the aabb overlap + btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); + } + } + + if (!multiProxy->m_bridgeProxies.size()) + { + ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision + ///this is needed to be able to calculate the aabb overlap + btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); + } +*/ + + + //update + for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) + { + btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; + bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); + } + +} +bool stopUpdating=false; + + + +class btMultiSapBroadphasePairSortPredicate +{ + public: + + bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) + { + btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0; + + return aProxy0 > bProxy0 || + (aProxy0 == bProxy0 && aProxy1 > bProxy1) || + (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); + } +}; + + + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb +void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +{ + +// m_simpleBroadphase->calculateOverlappingPairs(dispatcher); + + if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); + + // quicksort(overlappingPairArray,0,overlappingPairArray.size()); + + overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;i<overlappingPairArray.size();i++) + { + + btBroadphasePair& pair = overlappingPairArray[i]; + + btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0; + btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; + + bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); + + if (hasOverlap) + { + needsRemoval = false;//callback->processOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + m_invalidPair++; + gOverlappingPairs--; + } + + } + + ///if you don't like to skip the invalid pairs in the array, execute following code: + #define CLEAN_INVALID_PAIRS 1 + #ifdef CLEAN_INVALID_PAIRS + + //perform a sort, to sort 'invalid' pairs to the end + //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); + overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + #endif//CLEAN_INVALID_PAIRS + + //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); + } + + +} + + +bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) +{ + btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; + btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; + + return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, + multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); + +} + + +void btMultiSapBroadphase::printStats() +{ +/* printf("---------------------------------\n"); + + printf("btMultiSapBroadphase.h\n"); + printf("numHandles = %d\n",m_multiSapProxies.size()); + //find broadphase that contain this multiProxy + int numChildBroadphases = getBroadphaseArray().size(); + for (int i=0;i<numChildBroadphases;i++) + { + + btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; + childBroadphase->printStats(); + + } + */ + +} diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h new file mode 100644 index 00000000000..a0c002de856 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -0,0 +1,144 @@ +/* +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_MULTI_SAP_BROADPHASE +#define BT_MULTI_SAP_BROADPHASE + +#include "btBroadphaseInterface.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btOverlappingPairCache.h" + + +class btBroadphaseInterface; +class btSimpleBroadphase; + + +typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray; + +///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases. +///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time. +///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy. +///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328 +///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329 +class btMultiSapBroadphase :public btBroadphaseInterface +{ + btSapBroadphaseArray m_sapBroadphases; + + btSimpleBroadphase* m_simpleBroadphase; + + btOverlappingPairCache* m_overlappingPairs; + + class btQuantizedBvh* m_optimizedAabbTree; + + + bool m_ownsPairCache; + + btOverlapFilterCallback* m_filterCallback; + + int m_invalidPair; + + struct btBridgeProxy + { + btBroadphaseProxy* m_childProxy; + btBroadphaseInterface* m_childBroadphase; + }; + + +public: + + struct btMultiSapProxy : public btBroadphaseProxy + { + + ///array with all the entries that this proxy belongs to + btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies; + btVector3 m_aabbMin; + btVector3 m_aabbMax; + + int m_shapeType; + +/* void* m_userPtr; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; +*/ + btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) + :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax), + m_shapeType(shapeType) + { + m_multiSapParentProxy =this; + } + + + }; + +protected: + + + btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies; + +public: + + btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0); + + + btSapBroadphaseArray& getBroadphaseArray() + { + return m_sapBroadphases; + } + + const btSapBroadphaseArray& getBroadphaseArray() const + { + return m_sapBroadphases; + } + + virtual ~btMultiSapBroadphase(); + + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); + + void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase); + + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + + bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + virtual btOverlappingPairCache* getOverlappingPairCache() + { + return m_overlappingPairs; + } + virtual const btOverlappingPairCache* getOverlappingPairCache() const + { + return m_overlappingPairs; + } + + ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame + ///will add some transform later + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin.setValue(-1e30f,-1e30f,-1e30f); + aabbMax.setValue(1e30f,1e30f,1e30f); + } + + void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); + + virtual void printStats(); + + void quicksort (btBroadphasePairArray& a, int lo, int hi); + +}; + +#endif //BT_MULTI_SAP_BROADPHASE diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 60f0a41a9d7..ff65cdde79f 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -1,4 +1,3 @@ - /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ @@ -21,66 +20,415 @@ subject to the following restrictions: #include "btDispatcher.h" #include "btCollisionAlgorithm.h" +#include <stdio.h> + int gOverlappingPairs = 0; -btOverlappingPairCache::btOverlappingPairCache(): -m_blockedForChanges(false), -m_overlapFilterCallback(0) -//m_NumOverlapBroadphasePair(0) +int gRemovePairs =0; +int gAddedPairs =0; +int gFindPairs =0; + + + + +btHashedOverlappingPairCache::btHashedOverlappingPairCache(): + m_overlapFilterCallback(0), + m_blockedForChanges(false) { + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); } -btOverlappingPairCache::~btOverlappingPairCache() + + +btHashedOverlappingPairCache::~btHashedOverlappingPairCache() { //todo/test: show we erase/delete data, or is it automatic } -void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair) + +void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) { - - int findIndex = m_overlappingPairArray.findLinearSearch(findPair); - if (findIndex < m_overlappingPairArray.size()) + if (pair.m_algorithm) + { + { + pair.m_algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + } + } +} + + + + +void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + + class CleanPairCallback : public btOverlapCallback + { + btBroadphaseProxy* m_cleanProxy; + btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; + + public: + CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + if ((pair.m_pProxy0 == m_cleanProxy) || + (pair.m_pProxy1 == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + + + +void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + + class RemovePairCallback : public btOverlapCallback { - gOverlappingPairs--; - btBroadphasePair& pair = m_overlappingPairArray[findIndex]; - cleanOverlappingPair(pair); + btBroadphaseProxy* m_obsoleteProxy; + + public: + RemovePairCallback(btBroadphaseProxy* obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + return ((pair.m_pProxy0 == m_obsoleteProxy) || + (pair.m_pProxy1 == m_obsoleteProxy)); + } - m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1); + }; + + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + + + + + +btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) +{ + gFindPairs++; + if(proxy0>proxy1) btSwap(proxy0,proxy1); + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if (index == BT_NULL_PAIR) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include <stdio.h> + +void btHashedOverlappingPairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_NULL_PAIR; + } + + for(i=0;i<curHashtableSize;i++) + { + + const btBroadphasePair& pair = m_overlappingPairArray[i]; + int proxyId1 = pair.m_pProxy0->getUid(); + int proxyId2 = pair.m_pProxy1->getUid(); + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) +{ + if(proxy0>proxy1) btSwap(proxy0,proxy1); + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair != NULL) + { + return pair; + } + /*for(int i=0;i<m_overlappingPairArray.size();++i) + { + if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&& + (m_overlappingPairArray[i].m_pProxy1==proxy1)) + { + printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2); + internalFindPair(proxy0, proxy1, hash); + } + }*/ + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + void* mem = &m_overlappingPairArray.expand(); + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + } + + pair = new (mem) btBroadphasePair(*proxy0,*proxy1); +// pair->m_pProxy0 = proxy0; +// pair->m_pProxy1 = proxy1; + pair->m_algorithm = 0; + pair->m_userInfo = 0; + + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) +{ + gRemovePairs++; + if(proxy0>proxy1) btSwap(proxy0,proxy1); + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair == NULL) + { + return 0; + } + + cleanOverlappingPair(*pair,dispatcher); + + void* userData = pair->m_userInfo; + + btAssert(pair->m_pProxy0->getUid() == proxyId1); + btAssert(pair->m_pProxy1->getUid() == proxyId2); + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + btAssert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_NULL_PAIR); + + int previous = BT_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_NULL_PAIR) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { m_overlappingPairArray.pop_back(); + return userData; + } + + // Remove the last pair from the hash table. + const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + btAssert(index != BT_NULL_PAIR); + + previous = BT_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_NULL_PAIR) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return userData; +} +//#include <stdio.h> + +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +{ + + int i; + +// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); + for (i=0;i<m_overlappingPairArray.size();) + { + + btBroadphasePair* pair = &m_overlappingPairArray[i]; + if (callback->processOverlap(*pair)) + { + removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); + + gOverlappingPairs--; + } else + { + i++; + } } } -void btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair) + +void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher ) { - if (pair.m_algorithm) + if (!hasDeferredRemoval()) { + btBroadphasePair findPair(*proxy0,*proxy1); + + int findIndex = m_overlappingPairArray.findLinearSearch(findPair); + if (findIndex < m_overlappingPairArray.size()) { - delete pair.m_algorithm;; - pair.m_algorithm=0; + gOverlappingPairs--; + btBroadphasePair& pair = m_overlappingPairArray[findIndex]; + void* userData = pair.m_userInfo; + cleanOverlappingPair(pair,dispatcher); + + m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); + m_overlappingPairArray.pop_back(); + return userData; } } + + return 0; } -void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + + + +btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) { //don't add overlap with own assert(proxy0 != proxy1); if (!needsBroadphaseCollision(proxy0,proxy1)) - return; - - - btBroadphasePair pair(*proxy0,*proxy1); + return 0; - m_overlappingPairArray.push_back(pair); + void* mem = &m_overlappingPairArray.expand(); + btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); gOverlappingPairs++; + gAddedPairs++; + return pair; } @@ -88,7 +436,7 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation - btBroadphasePair* btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) { if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; @@ -109,18 +457,81 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa -void btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy) + + + + + +//#include <stdio.h> + +void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +{ + + int i; + + for (i=0;i<m_overlappingPairArray.size();) + { + + btBroadphasePair* pair = &m_overlappingPairArray[i]; + if (callback->processOverlap(*pair)) + { + cleanOverlappingPair(*pair,dispatcher); + + m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1); + m_overlappingPairArray.pop_back(); + gOverlappingPairs--; + } else + { + i++; + } + } +} + + + + +btSortedOverlappingPairCache::btSortedOverlappingPairCache(): + m_blockedForChanges(false), + m_hasDeferredRemoval(true), + m_overlapFilterCallback(0) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); +} + +btSortedOverlappingPairCache::~btSortedOverlappingPairCache() +{ + //todo/test: show we erase/delete data, or is it automatic +} + +void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +{ + if (pair.m_algorithm) + { + { + pair.m_algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + gRemovePairs--; + } + } +} + + +void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) { class CleanPairCallback : public btOverlapCallback { btBroadphaseProxy* m_cleanProxy; btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache) + CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :m_cleanProxy(cleanProxy), - m_pairCache(pairCache) + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } virtual bool processOverlap(btBroadphasePair& pair) @@ -128,22 +539,21 @@ void btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy) if ((pair.m_pProxy0 == m_cleanProxy) || (pair.m_pProxy1 == m_cleanProxy)) { - m_pairCache->cleanOverlappingPair(pair); + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); } return false; } }; - CleanPairCallback cleanPairs(proxy,this); + CleanPairCallback cleanPairs(proxy,this,dispatcher); - processAllOverlappingPairs(&cleanPairs); + processAllOverlappingPairs(&cleanPairs,dispatcher); } - -void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy) +void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) { class RemovePairCallback : public btOverlapCallback @@ -163,34 +573,7 @@ void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP }; - RemovePairCallback removeCallback(proxy); - processAllOverlappingPairs(&removeCallback); -} - - - -void btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback) -{ - - int i; - - for (i=0;i<m_overlappingPairArray.size();) - { - - btBroadphasePair* pair = &m_overlappingPairArray[i]; - if (callback->processOverlap(*pair)) - { - cleanOverlappingPair(*pair); - - m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - m_overlappingPairArray.pop_back(); - gOverlappingPairs--; - } else - { - i++; - } - } + processAllOverlappingPairs(&removeCallback,dispatcher); } - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index a81fe3264df..66679bd218a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -1,4 +1,3 @@ - /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ @@ -20,9 +19,13 @@ subject to the following restrictions: #include "btBroadphaseInterface.h" #include "btBroadphaseProxy.h" -#include "../../LinearMath/btPoint3.h" -#include "../../LinearMath/btAlignedObjectArray.h" +#include "btOverlappingPairCallback.h" + +#include "LinearMath/btPoint3.h" +#include "LinearMath/btAlignedObjectArray.h" +class btDispatcher; +typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray; struct btOverlapCallback { @@ -30,6 +33,7 @@ struct btOverlapCallback {} //return true for deletion of the pair virtual bool processOverlap(btBroadphasePair& pair) = 0; + }; struct btOverlapFilterCallback @@ -40,38 +44,261 @@ struct btOverlapFilterCallback virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0; }; -///btOverlappingPairCache maintains the objects with overlapping AABB + + + + + + +extern int gRemovePairs; +extern int gAddedPairs; +extern int gFindPairs; + +const int BT_NULL_PAIR=0xffffffff; + +///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases. +///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations. +class btOverlappingPairCache : public btOverlappingPairCallback +{ +public: + virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor + + virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; + + virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; + + virtual btBroadphasePairArray& getOverlappingPairArray() = 0; + + virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0; + + virtual int getNumOverlappingPairs() const = 0; + + virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0; + + virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + + virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; + + virtual bool hasDeferredRemoval() = 0; + +}; + +/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com +class btHashedOverlappingPairCache : public btOverlappingPairCache +{ + btBroadphasePairArray m_overlappingPairArray; + btOverlapFilterCallback* m_overlapFilterCallback; + bool m_blockedForChanges; + + +public: + btHashedOverlappingPairCache(); + virtual ~btHashedOverlappingPairCache(); + + + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); + + SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + gAddedPairs++; + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + return internalAddPair(proxy0,proxy1); + } + + + + void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + + virtual btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); + + + + btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); + + int GetCount() const { return m_overlappingPairArray.size(); } +// btBroadphasePair* GetPairs() { return m_pairs; } + + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + void growTables(); + + SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2) + { + return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2; + } + + /* + // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm + // This assumes proxyId1 and proxyId2 are 16-bit. + SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2) + { + int key = (proxyId2 << 16) | proxyId1; + key = ~key + (key << 15); + key = key ^ (key >> 12); + key = key + (key << 2); + key = key ^ (key >> 4); + key = key * 2057; + key = key ^ (key >> 16); + return key; + } + */ + + + + SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) + { + int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast<unsigned int>(key); + } + + + + + + SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) + { + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. + if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2); + #endif + + int index = m_hashTable[hash]; + + while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if ( index == BT_NULL_PAIR ) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + virtual bool hasDeferredRemoval() + { + return false; + } + +public: + + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; + +}; + + + + +///btSortedOverlappingPairCache maintains the objects with overlapping AABB ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase -class btOverlappingPairCache : public btBroadphaseInterface +class btSortedOverlappingPairCache : public btOverlappingPairCache { protected: //avoid brute-force finding all the time - btAlignedObjectArray<btBroadphasePair> m_overlappingPairArray; - + btBroadphasePairArray m_overlappingPairArray; + //during the dispatch, check that user doesn't destroy/create proxy bool m_blockedForChanges; + + ///by default, do the removal during the pair traversal + bool m_hasDeferredRemoval; //if set, use the callback instead of the built in filter in needBroadphaseCollision btOverlapFilterCallback* m_overlapFilterCallback; + public: - btOverlappingPairCache(); - virtual ~btOverlappingPairCache(); + btSortedOverlappingPairCache(); + virtual ~btSortedOverlappingPairCache(); - virtual void processAllOverlappingPairs(btOverlapCallback*); + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); - void removeOverlappingPair(btBroadphasePair& pair); + void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); - void cleanOverlappingPair(btBroadphasePair& pair); + void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - void addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - void cleanProxyFromPairs(btBroadphaseProxy* proxy); + void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy); + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const @@ -84,10 +311,19 @@ class btOverlappingPairCache : public btBroadphaseInterface return collides; } - + + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + - virtual void refreshOverlappingPairs() =0; btBroadphasePair* getOverlappingPairArrayPtr() { @@ -114,7 +350,88 @@ class btOverlappingPairCache : public btBroadphaseInterface m_overlapFilterCallback = callback; } + virtual bool hasDeferredRemoval() + { + return m_hasDeferredRemoval; + } + + +}; + + + +///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and testing. +class btNullPairCache : public btOverlappingPairCache +{ + + btBroadphasePairArray m_overlappingPairArray; + +public: + + virtual btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) + { + + } + + virtual int getNumOverlappingPairs() const + { + return 0; + } + + virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) + { + + } + + virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) + { + } + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) + { + } + + virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/) + { + return 0; + } + + virtual bool hasDeferredRemoval() + { + return true; + } + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) + { + return 0; + } + + virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/) + { + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + { + } + + }; + + #endif //OVERLAPPING_PAIR_CACHE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h new file mode 100644 index 00000000000..9c7b6f81367 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h @@ -0,0 +1,40 @@ + +/* +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 OVERLAPPING_PAIR_CALLBACK_H +#define OVERLAPPING_PAIR_CALLBACK_H + +class btDispatcher; +struct btBroadphasePair; + +///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. +class btOverlappingPairCallback +{ +public: + virtual ~btOverlappingPairCallback() + { + + } + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0; + + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0; + + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0; + +}; + +#endif //OVERLAPPING_PAIR_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp new file mode 100644 index 00000000000..a30bd1fd9e1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -0,0 +1,1025 @@ +/* +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 "btQuantizedBvh.h" + +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btIDebugDraw.h" + + +btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), + //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) + m_traversalMode(TRAVERSAL_STACKLESS) + //m_traversalMode(TRAVERSAL_RECURSIVE) + ,m_subtreeHeaderCount(0) //PCK: add this line +{ + +} + + + + + +void btQuantizedBvh::buildInternal() +{ + ///assumes that caller filled in the m_quantizedLeafNodes + m_useQuantization = true; + int numLeafNodes = 0; + + if (m_useQuantization) + { + //now we have an array of leafnodes in m_leafNodes + numLeafNodes = m_quantizedLeafNodes.size(); + + m_quantizedContiguousNodes.resize(2*numLeafNodes); + + } + + m_curNodeIndex = 0; + + 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(); + } + + //PCK: update the copy of the size + m_subtreeHeaderCount = m_SubtreeHeaders.size(); + + //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary + m_quantizedLeafNodes.clear(); + m_leafNodes.clear(); +} + + + +///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 btQuantizedBvh::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(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; + m_useQuantization = true; +} + + + + +btQuantizedBvh::~btQuantizedBvh() +{ +} + +#ifdef DEBUG_TREE_BUILDING +int gStackDepth = 0; +int gMaxStackDepth = 0; +#endif //DEBUG_TREE_BUILDING + +void btQuantizedBvh::buildTree (int startIndex,int endIndex) +{ +#ifdef DEBUG_TREE_BUILDING + gStackDepth++; + if (gStackDepth > gMaxStackDepth) + gMaxStackDepth = gStackDepth; +#endif //DEBUG_TREE_BUILDING + + + int splitAxis, splitIndex, i; + int numIndices =endIndex-startIndex; + int curIndex = m_curNodeIndex; + + assert(numIndices>0); + + if (numIndices==1) + { +#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(startIndex,endIndex); + + splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); + + int internalNodeIndex = m_curNodeIndex; + + setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin); + setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax); + + for (i=startIndex;i<endIndex;i++) + { + mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i)); + } + + m_curNodeIndex++; + + + //internalNode->m_escapeIndex; + + 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); + +} + +void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +{ + btAssert(m_useQuantization); + + btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; + int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); + int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode)); + + btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; + int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); + int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast<int>(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; + } + + //PCK: update the copy of the size + m_subtreeHeaderCount = m_SubtreeHeaders.size(); +} + + +int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + btScalar splitValue; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); + means+=center; + } + 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 = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); + if (center[splitAxis] > splitValue) + { + //swap + swapLeafNodes(i,splitIndex); + splitIndex++; + } + } + + //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)); + (void)unbal; + btAssert(!unbal); + + return splitIndex; +} + + +int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) +{ + int i; + + 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 = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); + means+=center; + } + means *= (btScalar(1.)/(btScalar)numIndices); + + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); + btVector3 diff2 = center-means; + diff2 = diff2 * diff2; + variance += diff2; + } + variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + + return variance.maxAxis(); +} + + + +void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) + + if (m_useQuantization) + { + ///quantize query AABB + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0); + quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1); + + 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 btQuantizedBvh::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 isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + 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; + + //PCK: unsigned instead of bool + if (isLeafNode && (aabbOverlap != 0)) + { + nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); + } + + //PCK: unsigned instead of bool + if ((aabbOverlap != 0) || 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 btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); + if (aabbOverlap) + { + isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); + if (isLeafNode) + { + nodeCallback->processNode(rootNode); + } else + { + walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax); + walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax); + } + } + +} +*/ + +void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + //PCK: unsigned instead of bool + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); + isLeafNode = currentNode->isLeafNode(); + + //PCK: unsigned instead of bool + if (aabbOverlap != 0) + { + if (isLeafNode) + { + nodeCallback->processNode(currentNode->getPartId(),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 btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned boxBoxOverlap = 0; + unsigned rayBoxOverlap = 0; + + btScalar lambda_max = 1.0; +#define RAYAABB2 +#ifdef RAYAABB2 + btVector3 rayFrom = raySource; + btVector3 rayDirection = (rayTarget-raySource); + rayDirection.normalize (); + lambda_max = rayDirection.dot(rayTarget-raySource); + ///what about division by zero? --> just set rayDirection[i] to 1.0 + rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0]; + rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1]; + rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2]; + unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; +#endif + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0); + quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1); + + 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 < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + // only interested if this is closer than any previous hit + btScalar param = 1.0; + rayBoxOverlap = 0; + boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + if (boxBoxOverlap) + { + btVector3 bounds[2]; + bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); + bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; + btVector3 normal; +#if 0 + bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); + bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal); + if (ra2 != ra) + { + printf("functions don't match\n"); + } +#endif +#ifdef RAYAABB2 + ///careful with this check: need to check division by zero (above) and fix the unQuantize method + ///thanks Joerg/hiker for the reproduction case! + ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 + + rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); +#else + rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); +#endif + } + + if (isLeafNode && rayBoxOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((rayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + 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 < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + + if (isLeafNode && aabbOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((aabbOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +//This traversal can be called from Playstation 3 SPU +void btQuantizedBvh::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]; + + //PCK: unsigned instead of bool + unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap != 0) + { + walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, + subtree.m_rootNodeIndex, + subtree.m_rootNodeIndex+subtree.m_subtreeSize); + } + } +} + + +void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const +{ + bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (fast_path) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex); + } else { + /* Otherwise fallback to AABB overlap test */ + btVector3 aabbMin = raySource; + btVector3 aabbMax = raySource; + aabbMin.setMin(rayTarget); + aabbMax.setMax(rayTarget); + reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax); + } +} + + +void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const +{ + bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (fast_path) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } else { + /* Slow path: + Construct the bounding box for the entire box cast and send that down the tree */ + btVector3 qaabbMin = raySource; + btVector3 qaabbMax = raySource; + qaabbMin.setMin(rayTarget); + qaabbMax.setMax(rayTarget); + qaabbMin += aabbMin; + qaabbMax += aabbMax; + reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); + } +} + + +void btQuantizedBvh::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 btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +{ + if (m_useQuantization) + { + m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; + } else + { + m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; + } +} + +//PCK: include +#include <new> + +//PCK: consts +static const unsigned BVH_ALIGNMENT = 16; +static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; + +static const unsigned BVH_ALIGNMENT_BLOCKS = 2; + + + +unsigned int btQuantizedBvh::getAlignmentSerializationPadding() +{ + return BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; +} + +unsigned btQuantizedBvh::calculateSerializeBufferSize() +{ + unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding(); + baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + if (m_useQuantization) + { + return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode); + } + return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); +} + +bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) +{ + assert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); + m_subtreeHeaderCount = m_SubtreeHeaders.size(); + +/* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + { + ///check alignedment for buffer? + btAssert(0); + return false; + } +*/ + + btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer; + + // construct the class so the virtual function table, etc will be set up + // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor + new (targetBvh) btQuantizedBvh; + + if (i_swapEndian) + { + targetBvh->m_curNodeIndex = static_cast<int>(btSwapEndian(m_curNodeIndex)); + + + btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin); + btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax); + btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization); + + targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode); + targetBvh->m_subtreeHeaderCount = static_cast<int>(btSwapEndian(m_subtreeHeaderCount)); + } + else + { + targetBvh->m_curNodeIndex = m_curNodeIndex; + targetBvh->m_bvhAabbMin = m_bvhAabbMin; + targetBvh->m_bvhAabbMax = m_bvhAabbMax; + targetBvh->m_bvhQuantization = m_bvhQuantization; + targetBvh->m_traversalMode = m_traversalMode; + targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount; + } + + targetBvh->m_useQuantization = m_useQuantization; + + unsigned char *nodeData = (unsigned char *)targetBvh; + nodeData += sizeof(btQuantizedBvh); + + unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + int nodeCount = m_curNodeIndex; + + if (m_useQuantization) + { + targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast<int>(btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); + } + } + else + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]; + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]; + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex; + + + } + } + nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + } + else + { + targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex)); + targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart)); + targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex)); + } + } + else + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg; + targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg; + + targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex; + targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart; + targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex; + } + } + nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + } + + sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + // Now serialize the subtree headers + targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount); + if (i_swapEndian) + { + for (int i = 0; i < m_subtreeHeaderCount; i++) + { + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast<int>(btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex)); + targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast<int>(btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize)); + } + } + else + { + for (int i = 0; i < m_subtreeHeaderCount; i++) + { + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex); + targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize); + targetBvh->m_SubtreeHeaders[i] = m_SubtreeHeaders[i]; + } + } + + nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + + return true; +} + +btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +{ + + if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + { + return NULL; + } + btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer; + + if (i_swapEndian) + { + bvh->m_curNodeIndex = static_cast<int>(btSwapEndian(bvh->m_curNodeIndex)); + + btUnSwapVector3Endian(bvh->m_bvhAabbMin); + btUnSwapVector3Endian(bvh->m_bvhAabbMax); + btUnSwapVector3Endian(bvh->m_bvhQuantization); + + bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode); + bvh->m_subtreeHeaderCount = static_cast<int>(btSwapEndian(bvh->m_subtreeHeaderCount)); + } + + unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize(); + btAssert(calculatedBufSize <= i_dataBufferSize); + + if (calculatedBufSize > i_dataBufferSize) + { + return NULL; + } + + unsigned char *nodeData = (unsigned char *)bvh; + nodeData += sizeof(btQuantizedBvh); + + unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + int nodeCount = bvh->m_curNodeIndex; + + // Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor + // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor + new (bvh) btQuantizedBvh(*bvh, false); + + if (bvh->m_useQuantization) + { + bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast<int>(btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); + } + } + nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + } + else + { + bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex)); + bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart)); + bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex)); + } + } + nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + } + + sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + // Now serialize the subtree headers + bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount); + if (i_swapEndian) + { + for (int i = 0; i < bvh->m_subtreeHeaderCount; i++) + { + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast<int>(btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex)); + bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast<int>(btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize)); + } + } + + return bvh; +} + +// Constructor that prevents btVector3's default constructor from being called +btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : +m_bvhAabbMin(self.m_bvhAabbMin), +m_bvhAabbMax(self.m_bvhAabbMax), +m_bvhQuantization(self.m_bvhQuantization) +{ + + +} + + + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h new file mode 100644 index 00000000000..8a149b533fa --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -0,0 +1,486 @@ +/* +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 QUANTIZED_BVH_H +#define QUANTIZED_BVH_H + +//#define DEBUG_CHECK_DEQUANTIZATION 1 +#ifdef DEBUG_CHECK_DEQUANTIZATION +#ifdef __SPU__ +#define printf spu_printf +#endif //__SPU__ + +#include <stdio.h> +#include <stdlib.h> +#endif //DEBUG_CHECK_DEQUANTIZATION + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedAllocator.h" + + +//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp + + +//Note: currently we have 16 bytes per quantized node +#define MAX_SUBTREE_SIZE_IN_BYTES 2048 + +// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one +// actually) triangles each (since the sign bit is reserved +#define MAX_NUM_PARTS_IN_BITS 10 + +///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 +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //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()); + // Get only the lower bits where the triangle index is stored + return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); + } + int getPartId() const + { + btAssert(isLeafNode()); + // Get only the highest bits where the part index is stored + return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + } +} +; + +/// 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 +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //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: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //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]; + + btBvhSubtreeInfo() + { + //memset(&m_padding[0], 0, sizeof(m_padding)); + } + + + 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(int subPart, int triangleIndex) = 0; +}; + +#include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btAlignedObjectArray.h" + + + +///for code readability: +typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray; +typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray; +typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray; + + +///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. +///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase. +///It is recommended to use quantization for better performance and lower memory requirements. +ATTRIBUTE_ALIGNED16(class) btQuantizedBvh +{ +protected: + + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + + QuantizedNodeArray m_quantizedLeafNodes; + + QuantizedNodeArray m_quantizedContiguousNodes; + + int m_curNodeIndex; + + + //quantization data + bool m_useQuantization; + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + enum btTraversalMode + { + TRAVERSAL_STACKLESS = 0, + TRAVERSAL_STACKLESS_CACHE_FRIENDLY, + TRAVERSAL_RECURSIVE + }; +protected: + + btTraversalMode m_traversalMode; + + BvhSubtreeInfoArray m_SubtreeHeaders; + + //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray + int m_subtreeHeaderCount; + + + ///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) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; + + } + } + void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) + { + if (m_useQuantization) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); + } 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 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]; + quantize(quantizedAabbMin,newAabbMin,0); + quantize(quantizedAabbMax,newAabbMax,1); + 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: + + + + void buildTree (int startIndex,int endIndex); + + int calcSplittingAxis(int startIndex,int endIndex); + + int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); + + void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) 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; + + +#define USE_BANCHLESS 1 +#ifdef USE_BANCHLESS + //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) + SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const + { + return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0)); + } +#else + SIMD_FORCE_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; + } +#endif //USE_BANCHLESS + + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + +public: + btQuantizedBvh(); + + virtual ~btQuantizedBvh(); + + + ///***************************************** expert/internal use only ************************* + void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); + QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } + ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized + void buildInternal(); + ///***************************************** expert/internal use only ************************* + + void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; + void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const; + + SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const + { + + btAssert(m_useQuantization); + + btAssert(point.getX() <= m_bvhAabbMax.getX()); + btAssert(point.getY() <= m_bvhAabbMax.getY()); + btAssert(point.getZ() <= m_bvhAabbMax.getZ()); + + btAssert(point.getX() >= m_bvhAabbMin.getX()); + btAssert(point.getY() >= m_bvhAabbMin.getY()); + btAssert(point.getZ() >= m_bvhAabbMin.getZ()); + + btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; + ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative + ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) + ///todo: double-check this + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe)); + } + + +#ifdef DEBUG_CHECK_DEQUANTIZATION + btVector3 newPoint = unQuantize(out); + if (isMax) + { + if (newPoint.getX() < point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() < point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() < point.getZ()) + { + + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } else + { + if (newPoint.getX() > point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() > point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() > point.getZ()) + { + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } +#endif //DEBUG_CHECK_DEQUANTIZATION + + } + + + SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const + { + + btAssert(m_useQuantization); + + btVector3 clampedPoint(point2); + clampedPoint.setMax(m_bvhAabbMin); + clampedPoint.setMin(m_bvhAabbMax); + + quantize(out,clampedPoint,isMax); + + } + + SIMD_FORCE_INLINE btVector3 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; + } + + ///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; + } + + + SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; + } + + + SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() + { + return m_SubtreeHeaders; + } + + + /////Calculate space needed to store BVH for serialization + unsigned calculateSerializeBufferSize(); + + /// Data buffer MUST be 16 byte aligned + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); + + ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' + static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); + + static unsigned int getAlignmentSerializationPadding(); + + SIMD_FORCE_INLINE bool isQuantized() + { + return m_useQuantization; + } + +private: + // Special "copy" constructor that allows for in-place deserialization + // Prevents btVector3's default constructor from being called, but doesn't inialize much else + // ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need) + btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory); + +} +; + + +#endif //QUANTIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 30bcbe0c5f1..a57952ffa06 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -14,83 +14,84 @@ subject to the following restrictions: */ #include "btSimpleBroadphase.h" -#include <BulletCollision/BroadphaseCollision/btDispatcher.h> -#include <BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h> +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btMatrix3x3.h" #include <new> +extern int gOverlappingPairs; void btSimpleBroadphase::validate() { - for (int i=0;i<m_numProxies;i++) + for (int i=0;i<m_numHandles;i++) { - for (int j=i+1;j<m_numProxies;j++) + for (int j=i+1;j<m_numHandles;j++) { - assert(m_pProxies[i] != m_pProxies[j]); + btAssert(&m_pHandles[i] != &m_pHandles[j]); } } } -btSimpleBroadphase::btSimpleBroadphase(int maxProxies) - :btOverlappingPairCache(), - m_firstFreeProxy(0), - m_numProxies(0), - m_maxProxies(maxProxies) +btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache) + :m_pairCache(overlappingPairCache), + m_ownsPairCache(false), + m_invalidPair(0) { - m_proxies = new btSimpleBroadphaseProxy[maxProxies]; - m_freeProxies = new int[maxProxies]; - m_pProxies = new btSimpleBroadphaseProxy*[maxProxies]; + if (!overlappingPairCache) + { + void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16); + m_pairCache = new (mem)btHashedOverlappingPairCache(); + m_ownsPairCache = true; + } + + // allocate handles buffer and put all handles on free list + m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16); + m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies]; + m_maxHandles = maxProxies; + m_numHandles = 0; + m_firstFreeHandle = 0; - int i; - for (i=0;i<m_maxProxies;i++) { - m_freeProxies[i] = i; + for (int i = m_firstFreeHandle; i < maxProxies; i++) + { + m_pHandles[i].SetNextFree(i + 1); + m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes + } + m_pHandles[maxProxies - 1].SetNextFree(0); + } + } btSimpleBroadphase::~btSimpleBroadphase() { - delete[] m_proxies; - delete []m_freeProxies; - delete [] m_pProxies; + btAlignedFree(m_pHandlesRawPtr); - /*int i; - for (i=m_numProxies-1;i>=0;i--) + if (m_ownsPairCache) { - BP_Proxy* proxy = m_pProxies[i]; - destroyProxy(proxy); + m_pairCache->~btOverlappingPairCache(); + btAlignedFree(m_pairCache); } - */ } -btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask) +btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy) { - if (m_numProxies >= m_maxProxies) + if (m_numHandles >= m_maxHandles) { - assert(0); + btAssert(0); return 0; //should never happen, but don't let the game crash ;-) } - assert(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]); - - int freeIndex= m_freeProxies[m_firstFreeProxy]; - btSimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])btSimpleBroadphaseProxy(min,max,shapeType,userPtr,collisionFilterGroup,collisionFilterMask); - m_firstFreeProxy++; - - btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; - - int index = int(proxy - proxy1); - btAssert(index == freeIndex); + assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]); - m_pProxies[m_numProxies] = proxy; - m_numProxies++; - //validate(); + int newHandleIndex = allocHandle(); + btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy); return proxy; } @@ -124,34 +125,19 @@ protected: }; }; -void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg) +void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher) { - int i; - btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg); - btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; - - int index = int(proxy0 - proxy1); - btAssert (index < m_maxProxies); - m_freeProxies[--m_firstFreeProxy] = index; + freeHandle(proxy0); + + m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher); - removeOverlappingPairsContainingProxy(proxyOrg); - - for (i=0;i<m_numProxies;i++) - { - if (m_pProxies[i] == proxyOrg) - { - m_pProxies[i] = m_pProxies[m_numProxies-1]; - break; - } - } - m_numProxies--; //validate(); } -void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax) +void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/) { btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); sbp->m_min = aabbMin; @@ -186,37 +172,129 @@ public: } }; -void btSimpleBroadphase::refreshOverlappingPairs() +void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { //first check for new overlapping pairs int i,j; - for (i=0;i<m_numProxies;i++) + if (m_numHandles >= 0) { - btBroadphaseProxy* proxy0 = m_pProxies[i]; - for (j=i+1;j<m_numProxies;j++) + + for (i=0;i<m_numHandles;i++) { - btBroadphaseProxy* proxy1 = m_pProxies[j]; - btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); - btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); + btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i]; - if (aabbOverlap(p0,p1)) + for (j=i+1;j<m_numHandles;j++) { - if ( !findPair(proxy0,proxy1)) + btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j]; + btAssert(proxy0 != proxy1); + + btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); + btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); + + if (aabbOverlap(p0,p1)) + { + if ( !m_pairCache->findPair(proxy0,proxy1)) + { + m_pairCache->addOverlappingPair(proxy0,proxy1); + } + } else { - addOverlappingPair(proxy0,proxy1); + if (!m_pairCache->hasDeferredRemoval()) + { + if ( m_pairCache->findPair(proxy0,proxy1)) + { + m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher); + } + } } } - } - } + if (m_ownsPairCache && m_pairCache->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); - CheckOverlapCallback checkOverlap; + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - processAllOverlappingPairs(&checkOverlap); + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;i<overlappingPairArray.size();i++) + { + + btBroadphasePair& pair = overlappingPairArray[i]; + + bool isDuplicate = (pair == previousPair); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); + + if (hasOverlap) + { + needsRemoval = false;//callback->processOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + m_pairCache->cleanOverlappingPair(pair,dispatcher); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + m_invalidPair++; + gOverlappingPairs--; + } + + } + + ///if you don't like to skip the invalid pairs in the array, execute following code: +#define CLEAN_INVALID_PAIRS 1 +#ifdef CLEAN_INVALID_PAIRS + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; +#endif//CLEAN_INVALID_PAIRS + + } + } +} + + +bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); + btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); + return aabbOverlap(p0,p1); } + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index fb155e7047c..e2ebb825725 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -24,35 +24,69 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy { btVector3 m_min; btVector3 m_max; + int m_nextFree; + +// int m_handleId; + btSimpleBroadphaseProxy() {}; - btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask) - :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), + btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy) + :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy), m_min(minpt),m_max(maxpt) { (void)shapeType; } + + SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;} + SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;} + + + }; -///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks -class btSimpleBroadphase : public btOverlappingPairCache +///The SimpleBroadphase is just a unit-test for btAxisSweep3, bt32BitAxisSweep3, or btDbvtBroadphase, so use those classes instead. +///It is a brute force aabb culling broadphase based on O(n^2) aabb checks +class btSimpleBroadphase : public btBroadphaseInterface { protected: - btSimpleBroadphaseProxy* m_proxies; - int* m_freeProxies; - int m_firstFreeProxy; - - btSimpleBroadphaseProxy** m_pProxies; - int m_numProxies; + int m_numHandles; // number of active handles + int m_maxHandles; // max number of handles + + btSimpleBroadphaseProxy* m_pHandles; // handles pool + void* m_pHandlesRawPtr; + int m_firstFreeHandle; // free handles list + int allocHandle() + { + btAssert(m_numHandles < m_maxHandles); + int freeHandle = m_firstFreeHandle; + m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree(); + m_numHandles++; + return freeHandle; + } + + void freeHandle(btSimpleBroadphaseProxy* proxy) + { + int handle = int(proxy-m_pHandles); + btAssert(handle >= 0 && handle < m_maxHandles); + + proxy->SetNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + + m_numHandles--; + } + + btOverlappingPairCache* m_pairCache; + bool m_ownsPairCache; + + int m_invalidPair; - int m_maxProxies; inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) @@ -67,26 +101,48 @@ protected: protected: - virtual void refreshOverlappingPairs(); + + public: - btSimpleBroadphase(int maxProxies=16384); + btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0); virtual ~btSimpleBroadphase(); static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1); - virtual btBroadphaseProxy* createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - virtual void destroyProxy(btBroadphaseProxy* proxy); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); - - + btOverlappingPairCache* getOverlappingPairCache() + { + return m_pairCache; + } + const btOverlappingPairCache* getOverlappingPairCache() const + { + return m_pairCache; + } + bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame + ///will add some transform later + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin.setValue(-1e30f,-1e30f,-1e30f); + aabbMax.setValue(1e30f,1e30f,1e30f); + } + + virtual void printStats() + { +// printf("btSimpleBroadphase.h\n"); +// printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); + } }; diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt index e565bf7edea..d77ca6444c7 100644 --- a/extern/bullet2/src/BulletCollision/CMakeLists.txt +++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt @@ -5,56 +5,149 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src } ADD_LIBRARY(LibBulletCollision BroadphaseCollision/btAxisSweep3.cpp + BroadphaseCollision/btAxisSweep3.h BroadphaseCollision/btBroadphaseProxy.cpp + BroadphaseCollision/btBroadphaseProxy.h BroadphaseCollision/btCollisionAlgorithm.cpp + BroadphaseCollision/btCollisionAlgorithm.h BroadphaseCollision/btDispatcher.cpp + BroadphaseCollision/btDispatcher.h + BroadphaseCollision/btDbvtBroadphase.cpp + BroadphaseCollision/btDbvtBroadphase.h + BroadphaseCollision/btDbvt.cpp + BroadphaseCollision/btDbvt.h + BroadphaseCollision/btMultiSapBroadphase.cpp + BroadphaseCollision/btMultiSapBroadphase.h BroadphaseCollision/btOverlappingPairCache.cpp + BroadphaseCollision/btOverlappingPairCache.h + BroadphaseCollision/btOverlappingPairCallback.h + BroadphaseCollision/btQuantizedBvh.cpp + BroadphaseCollision/btQuantizedBvh.h BroadphaseCollision/btSimpleBroadphase.cpp + BroadphaseCollision/btSimpleBroadphase.h CollisionDispatch/btCollisionDispatcher.cpp + CollisionDispatch/btCollisionDispatcher.h CollisionDispatch/btCollisionObject.cpp + CollisionDispatch/btCollisionObject.h CollisionDispatch/btCollisionWorld.cpp + CollisionDispatch/btCollisionWorld.h CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btCompoundCollisionAlgorithm.h CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btConvexConcaveCollisionAlgorithm.h + CollisionDispatch/btDefaultCollisionConfiguration.cpp + CollisionDispatch/btDefaultCollisionConfiguration.h CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + CollisionDispatch/btSphereSphereCollisionAlgorithm.h + CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxCollisionAlgorithm.h + CollisionDispatch/btBoxBoxDetector.cpp + CollisionDispatch/btBoxBoxDetector.h CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + CollisionDispatch/btSphereBoxCollisionAlgorithm.h + CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + CollisionDispatch/btConvexPlaneCollisionAlgorithm.h + CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp + CollisionDispatch/btSphereTriangleCollisionAlgorithm.h CollisionDispatch/btConvexConvexAlgorithm.cpp + CollisionDispatch/btConvexConvexAlgorithm.h CollisionDispatch/btEmptyCollisionAlgorithm.cpp + CollisionDispatch/btEmptyCollisionAlgorithm.h CollisionDispatch/btManifoldResult.cpp + CollisionDispatch/btManifoldResult.h CollisionDispatch/btSimulationIslandManager.cpp + CollisionDispatch/btSimulationIslandManager.h CollisionDispatch/btUnionFind.cpp + CollisionDispatch/btUnionFind.h + CollisionDispatch/SphereTriangleDetector.cpp + CollisionDispatch/SphereTriangleDetector.h CollisionShapes/btBoxShape.cpp + CollisionShapes/btBoxShape.h CollisionShapes/btBvhTriangleMeshShape.cpp + CollisionShapes/btBvhTriangleMeshShape.h + CollisionShapes/btCapsuleShape.cpp + CollisionShapes/btCapsuleShape.h CollisionShapes/btCollisionShape.cpp + CollisionShapes/btCollisionShape.h CollisionShapes/btCompoundShape.cpp + CollisionShapes/btCompoundShape.h CollisionShapes/btConcaveShape.cpp + CollisionShapes/btConcaveShape.h CollisionShapes/btConeShape.cpp + CollisionShapes/btConeShape.h CollisionShapes/btConvexHullShape.cpp + CollisionShapes/btConvexHullShape.h CollisionShapes/btConvexShape.cpp + CollisionShapes/btConvexShape.h + CollisionShapes/btConvexInternalShape.cpp + CollisionShapes/btConvexInternalShape.h CollisionShapes/btConvexTriangleMeshShape.cpp + CollisionShapes/btConvexTriangleMeshShape.h CollisionShapes/btCylinderShape.cpp + CollisionShapes/btCylinderShape.h CollisionShapes/btEmptyShape.cpp + CollisionShapes/btEmptyShape.h + CollisionShapes/btHeightfieldTerrainShape.cpp + CollisionShapes/btHeightfieldTerrainShape.h CollisionShapes/btMinkowskiSumShape.cpp + CollisionShapes/btMinkowskiSumShape.h + CollisionShapes/btMaterial.h + CollisionShapes/btMultimaterialTriangleMeshShape.cpp + CollisionShapes/btMultimaterialTriangleMeshShape.h CollisionShapes/btMultiSphereShape.cpp + CollisionShapes/btMultiSphereShape.h CollisionShapes/btOptimizedBvh.cpp + CollisionShapes/btOptimizedBvh.h CollisionShapes/btPolyhedralConvexShape.cpp + CollisionShapes/btPolyhedralConvexShape.h + CollisionShapes/btScaledBvhTriangleMeshShape.cpp + CollisionShapes/btScaledBvhTriangleMeshShape.h CollisionShapes/btTetrahedronShape.cpp + CollisionShapes/btTetrahedronShape.h CollisionShapes/btSphereShape.cpp + CollisionShapes/btSphereShape.h + CollisionShapes/btShapeHull.h + CollisionShapes/btShapeHull.cpp CollisionShapes/btStaticPlaneShape.cpp + CollisionShapes/btStaticPlaneShape.h CollisionShapes/btStridingMeshInterface.cpp + CollisionShapes/btStridingMeshInterface.h CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleCallback.h CollisionShapes/btTriangleBuffer.cpp + CollisionShapes/btTriangleBuffer.h CollisionShapes/btTriangleIndexVertexArray.cpp + CollisionShapes/btTriangleIndexVertexArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.cpp CollisionShapes/btTriangleMesh.cpp + CollisionShapes/btTriangleMesh.h CollisionShapes/btTriangleMeshShape.cpp + CollisionShapes/btTriangleMeshShape.h + CollisionShapes/btUniformScalingShape.cpp + CollisionShapes/btUniformScalingShape.h NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btContinuousConvexCollision.h NarrowPhaseCollision/btGjkEpa.cpp + NarrowPhaseCollision/btGjkEpa.h + NarrowPhaseCollision/btGjkEpa2.cpp + NarrowPhaseCollision/btGjkEpa2.h NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h NarrowPhaseCollision/btConvexCast.cpp + NarrowPhaseCollision/btConvexCast.h NarrowPhaseCollision/btGjkConvexCast.cpp + NarrowPhaseCollision/btGjkConvexCast.h NarrowPhaseCollision/btGjkPairDetector.cpp + NarrowPhaseCollision/btGjkPairDetector.h NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h NarrowPhaseCollision/btPersistentManifold.cpp + NarrowPhaseCollision/btPersistentManifold.h NarrowPhaseCollision/btRaycastCallback.cpp + NarrowPhaseCollision/btRaycastCallback.h NarrowPhaseCollision/btSubSimplexConvexCast.cpp + NarrowPhaseCollision/btSubSimplexConvexCast.h NarrowPhaseCollision/btVoronoiSimplexSolver.cpp + NarrowPhaseCollision/btVoronoiSimplexSolver.h ) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 81133670f0c..f6c1e32ac7c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -26,7 +26,7 @@ m_triangle(triangle) } -void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) { (void)debugDraw; @@ -42,7 +42,16 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact)) { - output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + if (swapResults) + { + btVector3 normalOnB = transformB.getBasis()*normal; + btVector3 normalOnA = -normalOnB; + btVector3 pointOnA = transformB*point+normalOnB*depth; + output.addContactPoint(normalOnA,pointOnA,depth); + } else + { + output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + } } } @@ -53,6 +62,8 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res // See also geometrictools.com // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv +btScalar 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; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index b32806a6846..26dabaa480e 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 "../NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "../../LinearMath/btPoint3.h" +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "LinearMath/btPoint3.h" class btSphereShape; @@ -28,7 +28,7 @@ class btTriangleShape; /// sphere-triangle to match the btDiscreteCollisionDetectorInterface struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface { - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp new file mode 100644 index 00000000000..cd0c028012c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -0,0 +1,85 @@ +/* +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 "btBoxBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "btBoxBoxDetector.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); + m_ownManifold = true; + } +} + +btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + btCollisionObject* col0 = body0; + btCollisionObject* col1 = body1; + btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); + btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); + + + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); +#ifndef USE_PERSISTENT_CONTACTS + m_manifoldPtr->clearManifold(); +#endif //USE_PERSISTENT_CONTACTS + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = 1e30f; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); + + btBoxBoxDetector detector(box0,box1); + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + +#ifdef USE_PERSISTENT_CONTACTS + // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } +#endif //USE_PERSISTENT_CONTACTS + +} + +btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +{ + //not yet + return 1.f; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h new file mode 100644 index 00000000000..35afaf175a1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/* +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 BOX_BOX__COLLISION_ALGORITHM_H +#define BOX_BOX__COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; + +///box-box collision detection +class btBoxBoxCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btBoxBoxCollisionAlgorithm(); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + int bbsize = sizeof(btBoxBoxCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //BOX_BOX__COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp new file mode 100644 index 00000000000..45ebff5dc45 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -0,0 +1,683 @@ + +/* + * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is 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. +*/ + +///ODE box-box collision detection is adapted to work with Bullet + +#include "btBoxBoxDetector.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" + +#include <float.h> +#include <string.h> + +btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2) +: m_box1(box1), +m_box2(box2) +{ + +} + + +// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and +// generate contact points. this returns 0 if there is no contact otherwise +// it returns the number of contacts generated. +// `normal' returns the contact normal. +// `depth' returns the maximum penetration depth along that normal. +// `return_code' returns a number indicating the type of contact that was +// detected: +// 1,2,3 = box 2 intersects with a face of box 1 +// 4,5,6 = box 1 intersects with a face of box 2 +// 7..15 = edge-edge contact +// `maxc' is the maximum number of contacts allowed to be generated, i.e. +// the size of the `contact' array. +// `contact' and `skip' are the contact array information provided to the +// collision functions. this function only fills in the position and depth +// fields. +struct dContactGeom; +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +#define dInfinity FLT_MAX + + +/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } +PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } +PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } +*/ +static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } +static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } +static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } +#define dMULTIPLYOP1_331(A,op,B,C) \ +{\ + (A)[0] op dDOT41((B),(C)); \ + (A)[1] op dDOT41((B+1),(C)); \ + (A)[2] op dDOT41((B+2),(C)); \ +} + +#define dMULTIPLYOP0_331(A,op,B,C) \ +{ \ + (A)[0] op dDOT((B),(C)); \ + (A)[1] op dDOT((B+4),(C)); \ + (A)[2] op dDOT((B+8),(C)); \ +} + +#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) + +typedef btScalar dMatrix3[4*3]; + +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta); +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta) +{ + btVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + btScalar uaub = dDOT(ua,ub); + btScalar q1 = dDOT(ua,p); + btScalar q2 = -dDOT(ub,p); + btScalar d = 1-uaub*uaub; + if (d <= btScalar(0.0001f)) { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else { + d = 1.f/d; + *alpha = (q1 + uaub*q2)*d; + *beta = (uaub*q1 + q2)*d; + } +} + + + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). + +static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq=4,nr=0; + btScalar buffer[16]; + btScalar *q = p; + btScalar *r = ret; + for (int dir=0; dir <= 1; dir++) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign=-1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + btScalar *pq = q; + btScalar *pr = r; + nr = 0; + for (int i=nq; i > 0; i--) { + // go through all points in q and all lines between adjacent points + if (sign*pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + btScalar *nextq = (i > 1) ? pq+2 : q; + if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q==ret) ? buffer : ret; + nq = nr; + } + } + done: + if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); + return nr; +} + + +#define M__PI 3.14159265f + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. + +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + int i,j; + btScalar a,cx,cy,q; + if (n==1) { + cx = p[0]; + cy = p[1]; + } + else if (n==2) { + cx = btScalar(0.5)*(p[0] + p[2]); + cy = btScalar(0.5)*(p[1] + p[3]); + } + else { + a = 0; + cx = 0; + cy = 0; + for (i=0; i<(n-1); i++) { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + a = 1.f/(btScalar(3.0)*(a+q)); + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + // compute the angle of each point w.r.t. the centroid + btScalar A[8]; + for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx); + + // search for points that have angles closest to A[i0] + i*(2*pi/m). + int avail[8]; + for (i=0; i<n; i++) avail[i] = 1; + avail[i0] = 0; + iret[0] = i0; + iret++; + for (j=1; j<m; j++) { + a = btScalar(j)*(2*M__PI/m) + A[i0]; + if (a > M__PI) a -= 2*M__PI; + btScalar maxdiff=1e9,diff; +#if defined(DEBUG) || defined (_DEBUG) + *iret = i0; // iret is not allowed to keep this value +#endif + for (i=0; i<n; i++) { + if (avail[i]) { + diff = btFabs (A[i]-a); + if (diff > M__PI) diff = 2*M__PI - diff; + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; + } + } + } +#if defined(DEBUG) || defined (_DEBUG) + btAssert (*iret != i0); // ensure iret got set +#endif + avail[*iret] = 0; + iret++; + } +} + + + +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) +{ + const btScalar fudge_factor = btScalar(1.05); + btVector3 p,pp,normalC; + const btScalar *normalR = 0; + btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; + int i,j,invert_normal,code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p = p2 - p1; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0]*btScalar(0.5); + A[1] = side1[1]*btScalar(0.5); + A[2] = side1[2]*btScalar(0.5); + B[0] = side2[0]*btScalar(0.5); + B[1] = side2[1]*btScalar(0.5); + B[2] = side2[2]*btScalar(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + + Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); + Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); + Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + +#define TST(expr1,expr2,norm,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis = u1,u2,u3 + TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); + TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); + TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + + // separating axis = v1,v2,v3 + TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); + TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); + TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. +#undef TST +#define TST(expr1,expr2,n1,n2,n3,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2*fudge_factor > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } \ + } + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); + TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); + TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); + TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); + TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); + TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); + TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); + +#undef TST + + if (!code) return 0; + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMULTIPLY0_331 (normal,R1,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + btVector3 pa; + btScalar sign; + for (i=0; i<3; i++) pa[i] = p1[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); + for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; + } + + // find a point pb on the intersecting edge of box 2 + btVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); + for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; + } + + btScalar alpha,beta; + btVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; + + dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + { + + //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); + //contact[0].depth = *depth; + btVector3 pointInWorld; + +#ifdef USE_CENTER_POINT + for (i=0; i<3; i++) + pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); + output.addContactPoint(-normal,pointInWorld,-*depth); +#else + output.addContactPoint(-normal,pb,-*depth); +#endif // + *return_code = code; + } + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; + if (code <= 3) { + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + btVector3 normal2,nr,anr; + if (code <= 3) { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + dMULTIPLY1_331 (nr,Rb,normal2); + anr[0] = btFabs (nr[0]); + anr[1] = btFabs (nr[1]); + anr[2] = btFabs (nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr,a1,a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + btVector3 center; + if (nr[lanr] < 0) { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; + } + else { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN,code1,code2; + if (code <= 3) codeN = code-1; else codeN = code-4; + if (codeN==0) { + code1 = 1; + code2 = 2; + } + else if (codeN==1) { + code1 = 0; + code2 = 2; + } + else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) + btScalar c1,c2,m11,m12,m21,m22; + c1 = dDOT14 (center,Ra+code1); + c2 = dDOT14 (center,Ra+code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44 (Ra+code1,Rb+a1); + m12 = dDOT44 (Ra+code1,Rb+a2); + m21 = dDOT44 (Ra+code2,Rb+a1); + m22 = dDOT44 (Ra+code2,Rb+a2); + { + btScalar k1 = m11*Sb[a1]; + btScalar k2 = m21*Sb[a1]; + btScalar k3 = m12*Sb[a2]; + btScalar k4 = m22*Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + btScalar rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + btScalar ret[16]; + int n = intersectRectQuad2 (rect,quad,ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + btScalar point[3*8]; // penetrating contact points + btScalar dep[8]; // depths for those points + btScalar det1 = 1.f/(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j=0; j < n; j++) { + btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + for (i=0; i<3; i++) point[cnum*3+i] = + center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); + if (dep[cnum] >= 0) { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + } + } + if (cnum < 1) return 0; // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) { + + //AddContactPoint... + + //dContactGeom *con = CONTACT(contact,skip*j); + //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; + //con->depth = dep[j]; + + btVector3 pointInWorld; + for (i=0; i<3; i++) + pointInWorld[i] = point[j*3+i] + pa[i]; + output.addContactPoint(-normal,pointInWorld,-dep[j]); + + } + } + else { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + btScalar maxdepth = dep[0]; + for (i=1; i<cnum; i++) { + if (dep[i] > maxdepth) { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints2 (cnum,ret,maxc,i1,iret); + + for (j=0; j < maxc; j++) { +// dContactGeom *con = CONTACT(contact,skip*j); + // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + // con->depth = dep[iret[j]]; + + btVector3 posInWorld; + for (i=0; i<3; i++) + posInWorld[i] = point[iret[j]*3+i] + pa[i]; + output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) +{ + + const btTransform& transformA = input.m_transformA; + const btTransform& transformB = input.m_transformB; + + int skip = 0; + dContactGeom *contact = 0; + + dMatrix3 R1; + dMatrix3 R2; + + for (int j=0;j<3;j++) + { + R1[0+4*j] = transformA.getBasis()[j].x(); + R2[0+4*j] = transformB.getBasis()[j].x(); + + R1[1+4*j] = transformA.getBasis()[j].y(); + R2[1+4*j] = transformB.getBasis()[j].y(); + + + R1[2+4*j] = transformA.getBasis()[j].z(); + R2[2+4*j] = transformB.getBasis()[j].z(); + + } + + + + btVector3 normal; + btScalar depth; + int return_code; + int maxc = 4; + + + dBoxBox2 (transformA.getOrigin(), + R1, + 2.f*m_box1->getHalfExtentsWithMargin(), + transformB.getOrigin(), + R2, + 2.f*m_box2->getHalfExtentsWithMargin(), + normal, &depth, &return_code, + maxc, contact, skip, + output + ); + +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h new file mode 100644 index 00000000000..605294d47bd --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -0,0 +1,44 @@ +/* + * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + +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 BOX_BOX_DETECTOR_H +#define BOX_BOX_DETECTOR_H + + +class btBoxShape; +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + + +/// btBoxBoxDetector wraps the ODE box-box collision detector +/// re-distributed under the Zlib license with permission from Russell L. Smith +struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface +{ + btBoxShape* m_box1; + btBoxShape* m_box2; + +public: + + btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2); + + virtual ~btBoxBoxDetector() {}; + + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + +}; + +#endif //BT_BOX_BOX_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h new file mode 100644 index 00000000000..fad770ac26d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -0,0 +1,47 @@ +/* +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_COLLISION_CONFIGURATION +#define BT_COLLISION_CONFIGURATION +struct btCollisionAlgorithmCreateFunc; + +class btStackAlloc; +class btPoolAllocator; + +///btCollisionConfiguration allows to configure Bullet collision detection +///stack allocator size, default collision algorithms and persistent manifold pool size +///todo: describe the meaning +class btCollisionConfiguration +{ + +public: + + virtual ~btCollisionConfiguration() + { + } + + ///memory pools + virtual btPoolAllocator* getPersistentManifoldPool() = 0; + + virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; + + virtual btStackAlloc* getStackAllocator() = 0; + + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; + +}; + +#endif //BT_COLLISION_CONFIGURATION + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index d51a59af7f0..c6728918d16 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef COLLISION_CREATE_FUNC #define COLLISION_CREATE_FUNC -#include "../../LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAlignedObjectArray.h" typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; class btCollisionAlgorithm; class btCollisionObject; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index b535fac6563..a031a9f9784 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -19,69 +19,39 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" + #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "LinearMath/btPoolAllocator.h" +#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" int gNumManifold = 0; +#ifdef BT_DEBUG #include <stdio.h> +#endif - -btCollisionDispatcher::btCollisionDispatcher(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); - m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc; - for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) - { - for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) - { - m_doubleDispatch[i][j] = m_emptyCreateFunc; - } - } -} -//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 (): +btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): m_count(0), - m_useIslands(true) + m_useIslands(true), + m_staticWarningReported(false), + m_collisionConfiguration(collisionConfiguration) { int i; setNearCallback(defaultNearCallback); - //default CreationFunctions, filling the m_doubleDispatch table - m_convexConvexCreateFunc = new btConvexConvexAlgorithm::CreateFunc; - m_convexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::CreateFunc; - m_swappedConvexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; - m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc; - m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc; - m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc; + m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); + + m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) { for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) { - m_doubleDispatch[i][j] = internalFindCreateFunc(i,j); + m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); assert(m_doubleDispatch[i][j]); } } @@ -89,8 +59,6 @@ btCollisionDispatcher::btCollisionDispatcher (): }; -#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION - void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) { @@ -99,12 +67,6 @@ void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int prox btCollisionDispatcher::~btCollisionDispatcher() { - delete m_convexConvexCreateFunc; - delete m_convexConcaveCreateFunc; - delete m_swappedConvexConcaveCreateFunc; - delete m_compoundCreateFunc; - delete m_swappedCompoundCreateFunc; - delete m_emptyCreateFunc; } btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) @@ -117,7 +79,18 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) btCollisionObject* body0 = (btCollisionObject*)b0; btCollisionObject* body1 = (btCollisionObject*)b1; - btPersistentManifold* manifold = new btPersistentManifold (body0,body1); + void* mem = 0; + + if (m_persistentManifoldPoolAllocator->getFreeCount()) + { + mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); + } else + { + mem = btAlignedAlloc(sizeof(btPersistentManifold),16); + + } + btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0); + manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); return manifold; @@ -137,13 +110,19 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) //printf("releaseManifold: gNumManifold %d\n",gNumManifold); clearManifold(manifold); - ///todo: this can be improved a lot, linear search might be slow part! - int findIndex = m_manifoldsPtr.findLinearSearch(manifold); - if (findIndex < m_manifoldsPtr.size()) + int findIndex = manifold->m_index1a; + btAssert(findIndex < m_manifoldsPtr.size()); + m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); + m_manifoldsPtr[findIndex]->m_index1a = findIndex; + m_manifoldsPtr.pop_back(); + + manifold->~btPersistentManifold(); + if (m_persistentManifoldPoolAllocator->validPtr(manifold)) { - m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); - m_manifoldsPtr.pop_back(); - delete manifold; + m_persistentManifoldPoolAllocator->freeMemory(manifold); + } else + { + btAlignedFree(manifold); } } @@ -152,99 +131,19 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold) { - -#ifdef USE_DISPATCH_REGISTRY_ARRAY btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = this; - ci.m_manifold = sharedManifold; - btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()] - ->CreateCollisionAlgorithm(ci,body0,body1); -#else - btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1); -#endif //USE_DISPATCH_REGISTRY_ARRAY - return algo; -} + ci.m_dispatcher1 = this; + ci.m_manifold = sharedManifold; + btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0,body1); -#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION - -btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1) -{ - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) - { - return m_convexConvexCreateFunc; - } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) - { - return m_convexConcaveCreateFunc; - } - - if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) - { - return m_swappedConvexConcaveCreateFunc; - } - - if (btBroadphaseProxy::isCompound(proxyType0)) - { - return m_compoundCreateFunc; - } else - { - if (btBroadphaseProxy::isCompound(proxyType1)) - { - return m_swappedCompoundCreateFunc; - } - } - - //failed to find an algorithm - return m_emptyCreateFunc; + return algo; } -#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION - -#ifndef USE_DISPATCH_REGISTRY_ARRAY -btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold) -{ - m_count++; - - btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = this; - - if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConvex() ) - { - return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1); - } - if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConcave()) - { - return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); - } - - if (body1->getCollisionShape()->isConvex() && body0->getCollisionShape()->isConcave()) - { - return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); - } - - if (body0->getCollisionShape()->isCompound()) - { - return new btCompoundCollisionAlgorithm(ci,body0,body1,false); - } else - { - if (body1->getCollisionShape()->isCompound()) - { - return new btCompoundCollisionAlgorithm(ci,body0,body1,true); - } - } - - //failed to find an algorithm - return new btEmptyAlgorithm(ci); - -} -#endif //USE_DISPATCH_REGISTRY_ARRAY bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1) { @@ -264,13 +163,19 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO bool needsCollision = true; - //broadphase filtering already deals with this - if ((body0->isStaticObject() || body0->isKinematicObject()) && - (body1->isStaticObject() || body1->isKinematicObject())) +#ifdef BT_DEBUG + if (!m_staticWarningReported) { - printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); + //broadphase filtering already deals with this + if ((body0->isStaticObject() || body0->isKinematicObject()) && + (body1->isStaticObject() || body1->isKinematicObject())) + { + m_staticWarningReported = true; + printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); + } } - +#endif //BT_DEBUG + if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; else if (!body0->checkCollideWith(body1)) @@ -286,23 +191,25 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO ///this is useful for the collision dispatcher. class btCollisionPairCallback : public btOverlapCallback { - btDispatcherInfo& m_dispatchInfo; + const btDispatcherInfo& m_dispatchInfo; btCollisionDispatcher* m_dispatcher; public: - btCollisionPairCallback(btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) + btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) :m_dispatchInfo(dispatchInfo), m_dispatcher(dispatcher) { } - btCollisionPairCallback& operator=(btCollisionPairCallback& other) + /*btCollisionPairCallback& operator=(btCollisionPairCallback& other) { m_dispatchInfo = other.m_dispatchInfo; m_dispatcher = other.m_dispatcher; return *this; } + */ + virtual ~btCollisionPairCallback() {} @@ -316,13 +223,14 @@ public: }; -void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo) + +void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) { //m_blockedForChanges = true; btCollisionPairCallback collisionCallback(dispatchInfo,this); - pairCache->processAllOverlappingPairs(&collisionCallback); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); //m_blockedForChanges = false; @@ -332,7 +240,7 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa //by default, Bullet will use this near callback -void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo) +void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) { btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; @@ -365,3 +273,26 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, } } + + +void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) +{ + if (m_collisionAlgorithmPoolAllocator->getFreeCount()) + { + return m_collisionAlgorithmPoolAllocator->allocate(size); + } + + //warn user for overflow? + return btAlignedAlloc(static_cast<size_t>(size), 16); +} + +void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) +{ + if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) + { + m_collisionAlgorithmPoolAllocator->freeMemory(ptr); + } else + { + btAlignedFree(ptr); + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index ca5aba8f01c..a9c9cd414c1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -16,17 +16,18 @@ subject to the following restrictions: #ifndef COLLISION__DISPATCHER_H #define COLLISION__DISPATCHER_H -#include "../BroadphaseCollision/btDispatcher.h" -#include "../NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "../CollisionDispatch/btManifoldResult.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" -#include "../../LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "LinearMath/btAlignedObjectArray.h" class btIDebugDraw; class btOverlappingPairCache; - +class btPoolAllocator; +class btCollisionConfiguration; #include "btCollisionCreateFunc.h" @@ -34,7 +35,7 @@ class btOverlappingPairCache; class btCollisionDispatcher; ///user can override this nearcallback for collision filtering and more finegrained control over collision detection -typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo); +typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); ///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. @@ -46,26 +47,22 @@ class btCollisionDispatcher : public btDispatcher btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; bool m_useIslands; + + bool m_staticWarningReported; btManifoldResult m_defaultManifoldResult; btNearCallback m_nearCallback; + btPoolAllocator* m_collisionAlgorithmPoolAllocator; + + btPoolAllocator* m_persistentManifoldPoolAllocator; + btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - btCollisionAlgorithmCreateFunc* internalFindCreateFunc(int proxyType0,int proxyType1); - //default CreationFunctions, filling the m_doubleDispatch table - btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; - btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + btCollisionConfiguration* m_collisionConfiguration; -#ifndef USE_DISPATCH_REGISTRY_ARRAY - btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0); -#endif //USE_DISPATCH_REGISTRY_ARRAY public: @@ -92,11 +89,7 @@ public: return m_manifoldsPtr[index]; } - ///the default constructor creates/register default collision algorithms, for convex, compound and concave shape support - btCollisionDispatcher (); - - ///a special constructor that doesn't create/register the default collision algorithms - btCollisionDispatcher(bool noDefaultAlgorithms); + btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionDispatcher(); @@ -114,7 +107,7 @@ public: virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1); - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo); + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ; void setNearCallback(btNearCallback nearCallback) { @@ -127,7 +120,26 @@ public: } //by default, Bullet will use this near callback - static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo); + static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); + + virtual void* allocateCollisionAlgorithm(int size); + + virtual void freeCollisionAlgorithm(void* ptr); + + btCollisionConfiguration* getCollisionConfiguration() + { + return m_collisionConfiguration; + } + + const btCollisionConfiguration* getCollisionConfiguration() const + { + return m_collisionConfiguration; + } + + void setCollisionConfiguration(btCollisionConfiguration* config) + { + m_collisionConfiguration = config; + } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index d4c0a4e8cb3..846c9b9b989 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -13,18 +13,27 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + #include "btCollisionObject.h" btCollisionObject::btCollisionObject() - : m_broadphaseHandle(0), + : m_anisotropicFriction(1.f,1.f,1.f), + m_hasAnisotropicFriction(false), + m_broadphaseHandle(0), m_collisionShape(0), - m_collisionFlags(0), + m_rootCollisionShape(0), + m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), + m_islandTag1(-1), + m_companionId(-1), m_activationState1(1), m_deactivationTime(btScalar(0.)), + m_friction(btScalar(0.5)), + m_restitution(btScalar(0.)), m_userObjectPointer(0), + m_internalType(CO_COLLISION_OBJECT), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdSquareMotionThreshold(btScalar(0.)), + m_ccdMotionThreshold(btScalar(0.)), m_checkCollideWith(false) { @@ -55,3 +64,4 @@ void btCollisionObject::activate(bool forceActivation) } + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 9fb6a67c4a3..8442868cf89 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,7 +27,8 @@ subject to the following restrictions: struct btBroadphaseProxy; class btCollisionShape; -#include "../../LinearMath/btMotionState.h" +#include "LinearMath/btMotionState.h" +#include "LinearMath/btAlignedAllocator.h" @@ -48,8 +49,16 @@ protected: //without destroying the continuous interpolated motion (which uses this interpolation velocities) btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; + btVector3 m_anisotropicFriction; + bool m_hasAnisotropicFriction; + btBroadphaseProxy* m_broadphaseHandle; btCollisionShape* m_collisionShape; + + ///m_rootCollisionShape is temporarily used to store the original collision shape + ///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes + ///If it is NULL, the m_collisionShape is not temporarily replaced. + btCollisionShape* m_rootCollisionShape; int m_collisionFlags; @@ -65,8 +74,9 @@ protected: ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer void* m_userObjectPointer; - ///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead. - void* m_internalOwner; + ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody etc. + ///do not assign your own m_internalType unless you write a new dynamics object class. + int m_internalType; ///time of impact calculation btScalar m_hitFraction; @@ -74,21 +84,23 @@ protected: ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: btScalar m_ccdSweptSphereRadius; - /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - btScalar m_ccdSquareMotionThreshold; + /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold + btScalar m_ccdMotionThreshold; /// If some object should have elaborate collision filtering by sub-classes bool m_checkCollideWith; char m_pad[7]; - virtual bool checkCollideWithOverride(btCollisionObject* co) + virtual bool checkCollideWithOverride(btCollisionObject* /* co */) { return true; } public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + enum CollisionFlags { CF_STATIC_OBJECT= 1, @@ -97,29 +109,49 @@ public: CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution) }; + enum CollisionObjectTypes + { + CO_COLLISION_OBJECT =1, + CO_RIGID_BODY, + CO_SOFT_BODY + }; - inline bool mergesSimulationIslands() const + SIMD_FORCE_INLINE bool mergesSimulationIslands() const { ///static objects, kinematic and object without contact response don't merge islands return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0); } + const btVector3& getAnisotropicFriction() const + { + return m_anisotropicFriction; + } + void setAnisotropicFriction(const btVector3& anisotropicFriction) + { + m_anisotropicFriction = anisotropicFriction; + m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f); + } + bool hasAnisotropicFriction() const + { + return m_hasAnisotropicFriction; + } + - inline bool isStaticObject() const { + SIMD_FORCE_INLINE bool isStaticObject() const { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; } - inline bool isKinematicObject() const + SIMD_FORCE_INLINE bool isKinematicObject() const { return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; } - inline bool isStaticOrKinematicObject() const + SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const { return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ; } - inline bool hasContactResponse() const { + SIMD_FORCE_INLINE bool hasContactResponse() const { return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0; } @@ -128,23 +160,38 @@ public: virtual ~btCollisionObject(); - void setCollisionShape(btCollisionShape* collisionShape) + virtual void setCollisionShape(btCollisionShape* collisionShape) { m_collisionShape = collisionShape; + m_rootCollisionShape = collisionShape; } - const btCollisionShape* getCollisionShape() const + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; } - btCollisionShape* getCollisionShape() + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { return m_collisionShape; } - + SIMD_FORCE_INLINE const btCollisionShape* getRootCollisionShape() const + { + return m_rootCollisionShape; + } + + SIMD_FORCE_INLINE btCollisionShape* getRootCollisionShape() + { + return m_rootCollisionShape; + } + ///Avoid using this internal API call + ///internalSetTemporaryCollisionShape is used to temporary replace the actual collision shape by a child collision shape. + void internalSetTemporaryCollisionShape(btCollisionShape* collisionShape) + { + m_collisionShape = collisionShape; + } int getActivationState() const { return m_activationState1;} @@ -186,14 +233,9 @@ public: } ///reserved for Bullet internal usage - void* getInternalOwner() + int getInternalType() const { - return m_internalOwner; - } - - const void* getInternalOwner() const - { - return m_internalOwner; + return m_internalType; } btTransform& getWorldTransform() @@ -243,6 +285,15 @@ public: m_interpolationWorldTransform = trans; } + void setInterpolationLinearVelocity(const btVector3& linvel) + { + m_interpolationLinearVelocity = linvel; + } + + void setInterpolationAngularVelocity(const btVector3& angvel) + { + m_interpolationAngularVelocity = angvel; + } const btVector3& getInterpolationLinearVelocity() const { @@ -307,16 +358,22 @@ public: m_ccdSweptSphereRadius = radius; } + btScalar getCcdMotionThreshold() const + { + return m_ccdMotionThreshold; + } + btScalar getCcdSquareMotionThreshold() const { - return m_ccdSquareMotionThreshold; + return m_ccdMotionThreshold*m_ccdMotionThreshold; } - /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - void setCcdSquareMotionThreshold(btScalar ccdSquareMotionThreshold) + + /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold + void setCcdMotionThreshold(btScalar ccdMotionThreshold) { - m_ccdSquareMotionThreshold = ccdSquareMotionThreshold; + m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold; } ///users can point to their objects, userPointer is not used by Bullet @@ -331,6 +388,7 @@ public: m_userObjectPointer = userPointer; } + inline bool checkCollideWith(btCollisionObject* co) { if (m_checkCollideWith) @@ -338,9 +396,6 @@ public: return true; } - - -} -; +}; #endif //COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index b49036a5b50..d8674a320a7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -4,8 +4,8 @@ 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, +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. @@ -18,59 +18,57 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" - +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" + #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" + //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" -btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache, int stackSize) +btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) :m_dispatcher1(dispatcher), m_broadphasePairCache(pairCache), -m_ownsDispatcher(false), -m_ownsBroadphasePairCache(false) +m_debugDrawer(0) { - m_stackAlloc = new btStackAlloc(stackSize); + m_stackAlloc = collisionConfiguration->getStackAllocator(); m_dispatchInfo.m_stackAllocator = m_stackAlloc; } btCollisionWorld::~btCollisionWorld() { - m_stackAlloc->destroy(); - delete m_stackAlloc; //clean up remaining objects int i; for (i=0;i<m_collisionObjects.size();i++) { btCollisionObject* collisionObject= m_collisionObjects[i]; - + btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) { // // only clear the cached algorithms // - getBroadphase()->cleanProxyFromPairs(bp); - getBroadphase()->destroyProxy(bp); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); + getBroadphase()->destroyProxy(bp,m_dispatcher1); } } - if (m_ownsDispatcher) - delete m_dispatcher1; - if (m_ownsBroadphasePairCache) - delete m_broadphasePairCache; } @@ -105,66 +103,105 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho type, collisionObject, collisionFilterGroup, - collisionFilterMask + collisionFilterMask, + m_dispatcher1,0 )) ; - + } +void btCollisionWorld::updateAabbs() +{ + BT_PROFILE("updateAabbs"); + + btTransform predictedTrans; + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + + //only update aabb of active objects + if (colObj->isActive()) + { + btPoint3 minAabb,maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + //need to increase the aabb for contact thresholds + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; + + //moving objects should be moderately sized, probably something wrong if not + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + { + bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + } else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + colObj->setActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe && m_debugDrawer) + { + reportMe = false; + 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"); + } + } + } + } + +} void btCollisionWorld::performDiscreteCollisionDetection() { - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - - BEGIN_PROFILE("perform Broadphase Collision Detection"); + BT_PROFILE("performDiscreteCollisionDetection"); + btDispatcherInfo& dispatchInfo = getDispatchInfo(); - //update aabb (of all moved objects) + updateAabbs(); - btVector3 aabbMin,aabbMax; - 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); + BT_PROFILE("calculateOverlappingPairs"); + m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); } - m_broadphasePairCache->refreshOverlappingPairs(); - - - END_PROFILE("perform Broadphase Collision Detection"); - - BEGIN_PROFILE("performDiscreteCollisionDetection"); btDispatcher* dispatcher = getDispatcher(); - if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache,dispatchInfo); - - END_PROFILE("performDiscreteCollisionDetection"); + { + BT_PROFILE("dispatchAllCollisionPairs"); + if (dispatcher) + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + } } + void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) { - - + + //bool removeFromBroadphase = false; - + { - + btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) { // // only clear the cached algorithms // - getBroadphase()->cleanProxyFromPairs(bp); - getBroadphase()->destroyProxy(bp); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); + getBroadphase()->destroyProxy(bp,m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } @@ -181,172 +218,412 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback,short int collisionFilterMask) + RayResultCallback& resultCallback) { - btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); + const btConvexShape* castShape = &pointShape; + + if (collisionShape->isConvex()) + { + btConvexCast::CastResult castResult; + castResult.m_fraction = resultCallback.m_closestHitFraction; + + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; +#define USE_SUBSIMPLEX_CONVEX_CAST 1 +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); +#else + //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST + + castResult.m_normal.normalize(); + btCollisionWorld::LocalRayResult localRayResult + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_fraction + ); + + bool normalInWorldSpace = true; + resultCallback.addSingleResult(localRayResult, normalInWorldSpace); + + } + } + } + } else { + if (collisionShape->isConcave()) + { + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btBvhTriangleMeshShape + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //ConvexCast::CastResult + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): + btTriangleRaycastCallback(from,to), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitFraction); + + bool normalInWorldSpace = false; + return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); + } + + }; + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + } else + { + btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; + + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //ConvexCast::CastResult + + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): + btTriangleRaycastCallback(from,to), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitFraction); + + bool normalInWorldSpace = false; + return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); + + + } + + }; + + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + + btVector3 rayAabbMinLocal = rayFromLocal; + rayAabbMinLocal.setMin(rayToLocal); + btVector3 rayAabbMaxLocal = rayFromLocal; + rayAabbMaxLocal.setMax(rayToLocal); - objectQuerySingle(&pointShape,rayFromTrans,rayToTrans, - collisionObject, - collisionShape, - colObjWorldTransform, - resultCallback,collisionFilterMask); + triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + } + } else { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->isCompound()) + { + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); + int i=0; + for (i=0;i<compoundShape->getNumChildShapes();i++) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childWorldTrans = colObjWorldTransform * childTrans; + // replace collision shape so that callback can determine the triangle + btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); + collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); + rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback); + // restore + collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); + } + } + } + } } -void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& rayFromTrans,const btTransform& rayToTrans, +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback,short int collisionFilterMask) + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { - - if (collisionShape->isConvex()) + { + btConvexCast::CastResult castResult; + castResult.m_allowedPenetration = allowedPenetration; + castResult.m_fraction = btScalar(1.);//?? + + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + + btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver); + //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); + //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); + + btConvexCast* castPtr = &convexCaster1; + + + + if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) { - btConvexCast::CastResult castResult; - castResult.m_fraction = btScalar(1.);//?? - - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); - //GjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); - - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (castResult.m_fraction < resultCallback.m_closestHitFraction) { - //add hit - if (castResult.m_normal.length2() > btScalar(0.0001)) - { - castResult.m_normal.normalize(); - if (castResult.m_fraction < resultCallback.m_closestHitFraction) - { - - btCollisionWorld::LocalRayResult localRayResult + castResult.m_normal.normalize(); + btCollisionWorld::LocalConvexResult localConvexResult ( - collisionObject, + collisionObject, 0, castResult.m_normal, + castResult.m_hitPoint, castResult.m_fraction ); - resultCallback.AddSingleResult(localRayResult); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); - } - } } } - else + } + } else { + if (collisionShape->isConcave()) + { + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { - - if (collisionShape->isConcave()) + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation + btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); + + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + { + btCollisionWorld::ConvexResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + if (hitFraction <= m_resultCallback->m_closestHitFraction) + { - btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; - - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + bool normalInWorldSpace = true; - //ConvexCast::CastResult - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback - { - btCollisionWorld::RayResultCallback* m_resultCallback; - btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; - - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - btTriangleRaycastCallback(from,to), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } - - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitFraction); - - return m_resultCallback->AddSingleResult(rayResult); - - - } - - }; + return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + } + return hitFraction; + } + }; - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); - rcb.m_hitFraction = resultCallback.m_closestHitFraction; + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); + } else + { + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation + btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); + + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + { + btCollisionWorld::ConvexResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } - btVector3 rayAabbMinLocal = rayFromLocal; - rayAabbMinLocal.setMin(rayToLocal); - btVector3 rayAabbMaxLocal = rayFromLocal; - rayAabbMaxLocal.setMax(rayToLocal); - triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); - - } else + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) { - //todo: use AABB tree or other BVH acceleration structure! - if (collisionShape->isCompound()) + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + if (hitFraction <= m_resultCallback->m_closestHitFraction) { - const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); - int i=0; - for (i=0;i<compoundShape->getNumChildShapes();i++) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - objectQuerySingle(castShape, rayFromTrans,rayToTrans, - collisionObject, - childCollisionShape, - childWorldTrans, - resultCallback, collisionFilterMask); - } + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); + bool normalInWorldSpace = false; + return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); } + return hitFraction; } + + }; + + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + + btVector3 rayAabbMinLocal = convexFromLocal; + rayAabbMinLocal.setMin(convexToLocal); + btVector3 rayAabbMaxLocal = convexFromLocal; + rayAabbMaxLocal.setMax(convexToLocal); + rayAabbMinLocal += boxMinLocal; + rayAabbMaxLocal += boxMaxLocal; + triangleMesh->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + } + } else { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->isCompound()) + { + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); + int i=0; + for (i=0;i<compoundShape->getNumChildShapes();i++) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childWorldTrans = colObjWorldTransform * childTrans; + // replace collision shape so that callback can determine the triangle + btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); + collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); + objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback, allowedPenetration); + // restore + collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); + } } + } + } } -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask) +void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const { - + btTransform rayFromTrans,rayToTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFromWorld); rayToTrans.setIdentity(); - + rayToTrans.setOrigin(rayToWorld); /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) - + int i; for (i=0;i<m_collisionObjects.size();i++) { + ///terminate further ray tests, once the closestHitFraction reached zero + if (resultCallback.m_closestHitFraction == btScalar(0.f)) + break; + btCollisionObject* collisionObject= m_collisionObjects[i]; //only perform raycast if filterMask matches - if(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { + if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btScalar hitLambda = resultCallback.m_closestHitFraction; btVector3 hitNormal; if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { @@ -355,7 +632,52 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r collisionObject->getCollisionShape(), collisionObject->getWorldTransform(), resultCallback); - } + } + } + + } + +} + +void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback) const +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans = convexFromWorld; + convexToTrans = convexToWorld; + btVector3 castShapeAabbMin, castShapeAabbMax; + /* Compute AABB that encompasses angular movement */ + { + btVector3 linVel, angVel; + btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransform R; + R.setIdentity (); + R.setRotation (convexFromTrans.getRotation()); + castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + } + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* collisionObject= m_collisionObjects[i]; + //only perform raycast if filterMask matches + if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + getDispatchInfo().m_allowedCcdPenetration); + } } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index b6d80233ab7..7557691a9a9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -68,12 +68,12 @@ class btStackAlloc; class btCollisionShape; class btConvexShape; 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 "../BroadphaseCollision/btOverlappingPairCache.h" -#include "../../LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "LinearMath/btAlignedObjectArray.h" ///CollisionWorld is interface and container for the collision detection class btCollisionWorld @@ -90,18 +90,22 @@ protected: btStackAlloc* m_stackAlloc; - btOverlappingPairCache* m_broadphasePairCache; - - bool m_ownsDispatcher; - bool m_ownsBroadphasePairCache; + btBroadphaseInterface* m_broadphasePairCache; + + btIDebugDraw* m_debugDrawer; + public: //this constructor doesn't own the dispatcher and paircache/broadphase - btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache, int stackSize = 2*1024*1024); + btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionWorld(); + void setBroadphase(btBroadphaseInterface* pairCache) + { + m_broadphasePairCache = pairCache; + } btBroadphaseInterface* getBroadphase() { @@ -110,7 +114,7 @@ public: btOverlappingPairCache* getPairCache() { - return m_broadphasePairCache; + return m_broadphasePairCache->getOverlappingPairCache(); } @@ -119,6 +123,24 @@ public: return m_dispatcher1; } + const btDispatcher* getDispatcher() const + { + return m_dispatcher1; + } + + virtual void updateAabbs(); + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + { + m_debugDrawer = debugDrawer; + } + + virtual btIDebugDraw* getDebugDrawer() + { + return m_debugDrawer; + } + + ///LocalShapeInfo gives extra information for complex shapes ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart struct LocalShapeInfo @@ -153,28 +175,43 @@ public: ///RayResultCallback is used to report new raycast results struct RayResultCallback { + btScalar m_closestHitFraction; + btCollisionObject* m_collisionObject; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + virtual ~RayResultCallback() { } - btScalar m_closestHitFraction; - bool HasHit() + bool hasHit() const { - return (m_closestHitFraction < btScalar(1.)); + return (m_collisionObject != 0); } RayResultCallback() - :m_closestHitFraction(btScalar(1.)) + :m_closestHitFraction(btScalar(1.)), + m_collisionObject(0), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { + bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; } - virtual btScalar AddSingleResult(LocalRayResult& rayResult) = 0; + + + virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0; }; struct ClosestRayResultCallback : public RayResultCallback { ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_collisionObject(0) + m_rayToWorld(rayToWorld) { } @@ -183,24 +220,121 @@ public: btVector3 m_hitNormalWorld; btVector3 m_hitPointWorld; - btCollisionObject* m_collisionObject; - - virtual btScalar AddSingleResult(LocalRayResult& rayResult) + + virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) { - -//caller already does the filter on the m_closestHitFraction - assert(rayResult.m_hitFraction <= m_closestHitFraction); + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); m_closestHitFraction = rayResult.m_hitFraction; m_collisionObject = rayResult.m_collisionObject; - m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + } m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); return rayResult.m_hitFraction; } }; - + struct LocalConvexResult + { + LocalConvexResult(btCollisionObject* hitCollisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + const btVector3& hitPointLocal, + btScalar hitFraction + ) + :m_hitCollisionObject(hitCollisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitPointLocal(hitPointLocal), + m_hitFraction(hitFraction) + { + } + + btCollisionObject* m_hitCollisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btVector3 m_hitPointLocal; + btScalar m_hitFraction; + }; + + ///RayResultCallback is used to report new raycast results + struct ConvexResultCallback + { + btScalar m_closestHitFraction; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + + ConvexResultCallback() + :m_closestHitFraction(btScalar(1.)), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) + { + } + + virtual ~ConvexResultCallback() + { + } + + bool hasHit() const + { + return (m_closestHitFraction < btScalar(1.)); + } + + + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; + } + + virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; + }; + + struct ClosestConvexResultCallback : public ConvexResultCallback + { + ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld) + :m_convexFromWorld(convexFromWorld), + m_convexToWorld(convexToWorld), + m_hitCollisionObject(0) + { + } + + btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_convexToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + btCollisionObject* m_hitCollisionObject; + + virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) + { +//caller already does the filter on the m_closestHitFraction + btAssert(convexResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = convexResult.m_hitFraction; + m_hitCollisionObject = convexResult.m_hitCollisionObject; + if (normalInWorldSpace) + { + m_hitNormalWorld = convexResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + } + m_hitPointWorld = convexResult.m_hitPointLocal; + return convexResult.m_hitFraction; + } + }; int getNumCollisionObjects() const { @@ -209,7 +343,12 @@ public: /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback, short int collisionFilterMask=-1); + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + + // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback + // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. + void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback) const; + /// 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. @@ -218,16 +357,16 @@ public: btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback, short int collisionFilterMask=-1); + RayResultCallback& resultCallback); /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest. static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback, short int collisionFilterMask=-1); + ConvexResultCallback& resultCallback, btScalar allowedPenetration); - void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1); + void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); btCollisionObjectArray& getCollisionObjectArray() { @@ -249,6 +388,11 @@ public: return m_dispatchInfo; } + const btDispatcherInfo& getDispatchInfo() const + { + return m_dispatchInfo; + } + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 92f4c8b28a6..535b61992b0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -16,11 +16,17 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" - +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAabbUtil2.h" btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) -:m_isSwapped(isSwapped) +:btCollisionAlgorithm(ci), +m_isSwapped(isSwapped), +m_sharedManifold(ci.m_manifold) { + m_ownsManifold = false; + btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; assert (colObj->getCollisionShape()->isCompound()); @@ -32,11 +38,17 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg m_childCollisionAlgorithms.resize(numChildren); for (i=0;i<numChildren;i++) { - btCollisionShape* childShape = compoundShape->getChildShape(i); - btCollisionShape* orgShape = colObj->getCollisionShape(); - colObj->setCollisionShape( childShape ); - m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj); - colObj->setCollisionShape( orgShape ); + if (compoundShape->getDynamicAabbTree()) + { + m_childCollisionAlgorithms[i] = 0; + } else + { + btCollisionShape* tmpShape = colObj->getCollisionShape(); + btCollisionShape* childShape = compoundShape->getChildShape(i); + colObj->internalSetTemporaryCollisionShape( childShape ); + m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold); + colObj->internalSetTemporaryCollisionShape( tmpShape ); + } } } @@ -47,10 +59,109 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() int i; for (i=0;i<numChildren;i++) { - delete m_childCollisionAlgorithms[i]; + if (m_childCollisionAlgorithms[i]) + { + m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + } } } + + + +struct btCompoundLeafCallback : btDbvt::ICollide +{ + +public: + + btCollisionObject* m_compoundColObj; + btCollisionObject* m_otherObj; + btDispatcher* m_dispatcher; + const btDispatcherInfo& m_dispatchInfo; + btManifoldResult* m_resultOut; + btCollisionAlgorithm** m_childCollisionAlgorithms; + btPersistentManifold* m_sharedManifold; + + + + + btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold) + :m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), + m_childCollisionAlgorithms(childCollisionAlgorithms), + m_sharedManifold(sharedManifold) + { + + } + + + void ProcessChildShape(btCollisionShape* childShape,int index) + { + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape()); + + + //backup + btTransform orgTrans = m_compoundColObj->getWorldTransform(); + btTransform orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(index); + btTransform newChildWorldTrans = orgTrans*childTrans ; + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1); + + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + + m_compoundColObj->setWorldTransform( newChildWorldTrans); + m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans); + + //the contactpoint is still projected back using the original inverted worldtrans + btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape(); + m_compoundColObj->internalSetTemporaryCollisionShape( childShape ); + + if (!m_childCollisionAlgorithms[index]) + m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold); + + m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut); + if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 worldAabbMin,worldAabbMax; + m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1)); + m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1)); + } + + //revert back transform + m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape); + m_compoundColObj->setWorldTransform( orgTrans ); + m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans); + } + } + void Process(const btDbvtNode* leaf) + { + int index = leaf->dataAsInt; + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape()); + btCollisionShape* childShape = compoundShape->getChildShape(index); + if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 worldAabbMin,worldAabbMax; + btTransform orgTrans = m_compoundColObj->getWorldTransform(); + btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax); + m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0)); + } + ProcessChildShape(childShape,index); + + } +}; + + + + + + void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { btCollisionObject* colObj = m_isSwapped? body1 : body0; @@ -59,33 +170,69 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt assert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); - //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps - //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals - //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: - //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 - //then use each overlapping node AABB against Tree0 - //and vise versa. + btDbvt* tree = compoundShape->getDynamicAabbTree(); + //use a dynamic aabb tree to cull potential child-overlaps + btCompoundLeafCallback callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); - int numChildren = m_childCollisionAlgorithms.size(); - int i; - for (i=0;i<numChildren;i++) + + if (tree) { - //temporarily exchange parent btCollisionShape with childShape, and recurse - btCollisionShape* childShape = compoundShape->getChildShape(i); - //backup - btTransform orgTrans = colObj->getWorldTransform(); - btCollisionShape* orgShape = colObj->getCollisionShape(); + btVector3 localAabbMin,localAabbMax; + btTransform otherInCompoundSpace; + otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform(); + otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + //process all children, that overlap with the given AABB bounds + tree->collideTV(tree->m_root,bounds,callback); + + } else + { + //iterate over all children, perform an AABB check inside ProcessChildShape + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + callback.ProcessChildShape(compoundShape->getChildShape(i),i); + } + } + + { + //iterate over all children, perform an AABB check inside ProcessChildShape + int numChildren = m_childCollisionAlgorithms.size(); + int i; + btManifoldArray manifoldArray; + + for (i=0;i<numChildren;i++) + { + if (m_childCollisionAlgorithms[i]) + { + btCollisionShape* childShape = compoundShape->getChildShape(i); + //if not longer overlapping, remove the algorithm + btTransform orgTrans = colObj->getWorldTransform(); + btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(i); + btTransform newChildWorldTrans = orgTrans*childTrans ; + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1); + + if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + m_childCollisionAlgorithms[i] = 0; + } + + } + + } + + - const btTransform& childTrans = compoundShape->getChildTransform(i); - //btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( orgTrans*childTrans ); - //the contactpoint is still projected back using the original inverted worldtrans - colObj->setCollisionShape( childShape ); - m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut); - //revert back - colObj->setCollisionShape( orgShape); - colObj->setWorldTransform( orgTrans ); } } @@ -117,20 +264,20 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* //backup btTransform orgTrans = colObj->getWorldTransform(); - btCollisionShape* orgShape = colObj->getCollisionShape(); - + const btTransform& childTrans = compoundShape->getChildTransform(i); //btTransform newChildWorldTrans = orgTrans*childTrans ; colObj->setWorldTransform( orgTrans*childTrans ); - colObj->setCollisionShape( childShape ); + btCollisionShape* tmpShape = colObj->getCollisionShape(); + colObj->internalSetTemporaryCollisionShape( childShape ); btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); if (frac<hitFraction) { hitFraction = frac; } //revert back - colObj->setCollisionShape( orgShape); + colObj->internalSetTemporaryCollisionShape( tmpShape); colObj->setWorldTransform( orgTrans); } return hitFraction; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 7091b233b46..624a3cf10f5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -16,22 +16,25 @@ subject to the following restrictions: #ifndef COMPOUND_COLLISION_ALGORITHM_H #define COMPOUND_COLLISION_ALGORITHM_H -#include "../BroadphaseCollision/btCollisionAlgorithm.h" -#include "../BroadphaseCollision/btDispatcher.h" -#include "../BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "../NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" class btDispatcher; -#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" -#include "../../LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAlignedObjectArray.h" +class btDispatcher; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -/// Place holder, not fully implemented yet class btCompoundCollisionAlgorithm : public btCollisionAlgorithm { btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; + + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; public: @@ -43,11 +46,22 @@ public: btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + int i; + for (i=0;i<m_childCollisionAlgorithms.size();i++) + { + if (m_childCollisionAlgorithms[i]) + m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); + } + } + struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btCompoundCollisionAlgorithm(ci,body0,body1,false); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); + return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,false); } }; @@ -55,7 +69,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btCompoundCollisionAlgorithm(ci,body0,body1,true); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); + return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,true); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 24ceacfd40d..6d28904cb03 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -29,7 +29,7 @@ subject to the following restrictions: btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) : btCollisionAlgorithm(ci), m_isSwapped(isSwapped), -m_btConvexTriangleCallback(ci.m_dispatcher,body0,body1,isSwapped) +m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) { } @@ -37,6 +37,13 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() { } +void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +{ + if (m_btConvexTriangleCallback.m_manifoldPtr) + { + manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr); + } +} btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): @@ -79,7 +86,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i //aabb filter is already applied! btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = m_dispatcher; + ci.m_dispatcher1 = m_dispatcher; btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); @@ -109,13 +116,11 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i { btTriangleShape tm(triangle[0],triangle[1],triangle[2]); tm.setMargin(m_collisionMarginTriangle); - btCollisionShape* tmpShape = ob->getCollisionShape(); - ob->setCollisionShape( &tm ); + ob->internalSetTemporaryCollisionShape( &tm ); - - btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); ///this should use the btDispatcher, so the actual registered algorithm is used // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); @@ -123,12 +128,11 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); - delete colAlgo; - ob->setCollisionShape( tmpShape ); - + colAlgo->~btCollisionAlgorithm(); + ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); + ob->internalSetTemporaryCollisionShape( tmpShape); } - } @@ -188,9 +192,10 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); + resultOut->refreshContactPoints(); } - + } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 4915b6c20c8..34b9a107be9 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 "../BroadphaseCollision/btCollisionAlgorithm.h" -#include "../BroadphaseCollision/btDispatcher.h" -#include "../BroadphaseCollision/btBroadphaseInterface.h" -#include "../CollisionShapes/btTriangleCallback.h" -#include "../NarrowPhaseCollision/btPersistentManifold.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" class btDispatcher; -#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/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. @@ -55,11 +55,11 @@ int m_triangleCount; void clearCache(); - inline const btVector3& getAabbMin() const + SIMD_FORCE_INLINE const btVector3& getAabbMin() const { return m_aabbMin; } - inline const btVector3& getAabbMax() const + SIMD_FORCE_INLINE const btVector3& getAabbMax() const { return m_aabbMax; } @@ -88,13 +88,16 @@ public: btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); + void clearCache(); struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); + return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); } }; @@ -102,7 +105,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); + return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 9105fe20b49..cb830f889be 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -15,7 +15,7 @@ subject to the following restrictions: #include "btConvexConvexAlgorithm.h" -#include <stdio.h> +//#include <stdio.h> #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" @@ -33,7 +33,6 @@ subject to the following restrictions: -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -48,26 +47,16 @@ subject to the following restrictions: -btConvexConvexAlgorithm::CreateFunc::CreateFunc() -{ - m_ownsSolvers = true; - m_simplexSolver = new btVoronoiSimplexSolver(); - m_pdSolver = new btGjkEpaPenetrationDepthSolver; -} + btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) { - m_ownsSolvers = false; m_simplexSolver = simplexSolver; 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) @@ -152,6 +141,11 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #endif + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index cbea9a92b75..a0030e6793d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -16,12 +16,13 @@ subject to the following restrictions: #ifndef CONVEX_CONVEX_ALGORITHM_H #define CONVEX_CONVEX_ALGORITHM_H -#include "../BroadphaseCollision/btCollisionAlgorithm.h" -#include "../NarrowPhaseCollision/btGjkPairDetector.h" -#include "../NarrowPhaseCollision/btPersistentManifold.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" -#include "../NarrowPhaseCollision/btVoronoiSimplexSolver.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 "btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" class btConvexPenetrationDepthSolver; @@ -46,6 +47,14 @@ public: virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + ///should we use m_ownManifold to avoid adding duplicates? + if (m_manifoldPtr && m_ownManifold) + manifoldArray.push_back(m_manifoldPtr); + } + + void setLowLevelOfDetail(bool useLowLevel); @@ -58,15 +67,15 @@ public: { btConvexPenetrationDepthSolver* m_pdSolver; btSimplexSolverInterface* m_simplexSolver; - bool m_ownsSolvers; CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - CreateFunc(); + virtual ~CreateFunc(); virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); + return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp new file mode 100644 index 00000000000..391cf6c7bc4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -0,0 +1,108 @@ +/* +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 "btConvexPlaneCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" + +//#include <stdio.h> + +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ + btCollisionObject* convexObj = m_isSwapped? col1 : col0; + btCollisionObject* planeObj = m_isSwapped? col0 : col1; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); + m_ownManifold = true; + } +} + + +btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + + + +void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + if (!m_manifoldPtr) + return; + + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + btTransform planeInConvex; + planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; + + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + } + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } +} + +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h new file mode 100644 index 00000000000..7b258554171 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.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 CONVEX_PLANE_COLLISION_ALGORITHM_H +#define CONVEX_PLANE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + +public: + + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + + virtual ~btConvexPlaneCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp new file mode 100644 index 00000000000..1c317080544 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -0,0 +1,291 @@ +/* +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 "btDefaultCollisionConfiguration.h" + +#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM +#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" + + + +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btPoolAllocator.h" + + + + + +btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) +//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) +{ + + void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16); + m_simplexSolver = new (mem)btVoronoiSimplexSolver(); + +#define USE_EPA 1 +#ifdef USE_EPA + mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); + m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; +#else + mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); + m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; +#endif//USE_EPA + + + //default CreationFunctions, filling the m_doubleDispatch table + mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); + m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); + m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); + m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); + m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); + m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); + m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); + m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); + m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); + m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; + m_boxSphereCF->m_swapped = true; +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); + m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); + m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + m_triangleSphereCF->m_swapped = true; + + mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16); + m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc; + + //convex versus plane + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + m_planeConvexCF->m_swapped = true; + + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool + int maxSize = sizeof(btConvexConvexAlgorithm); + int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); + int maxSize3 = sizeof(btCompoundCollisionAlgorithm); + int maxSize4 = sizeof(btEmptyAlgorithm); + + int collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); + + if (constructionInfo.m_stackAlloc) + { + m_ownsStackAllocator = false; + this->m_stackAlloc = constructionInfo.m_stackAlloc; + } else + { + m_ownsStackAllocator = true; + void* mem = btAlignedAlloc(sizeof(btStackAlloc),16); + m_stackAlloc = new(mem)btStackAlloc(constructionInfo.m_defaultStackAllocatorSize); + } + + if (constructionInfo.m_persistentManifoldPool) + { + m_ownsPersistentManifoldPool = false; + m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; + } else + { + m_ownsPersistentManifoldPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); + } + + if (constructionInfo.m_collisionAlgorithmPool) + { + m_ownsCollisionAlgorithmPool = false; + m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; + } else + { + m_ownsCollisionAlgorithmPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + } + + +} + +btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() +{ + if (m_ownsStackAllocator) + { + m_stackAlloc->destroy(); + m_stackAlloc->~btStackAlloc(); + btAlignedFree(m_stackAlloc); + } + if (m_ownsCollisionAlgorithmPool) + { + m_collisionAlgorithmPool->~btPoolAllocator(); + btAlignedFree(m_collisionAlgorithmPool); + } + if (m_ownsPersistentManifoldPool) + { + m_persistentManifoldPool->~btPoolAllocator(); + btAlignedFree(m_persistentManifoldPool); + } + + m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexConvexCreateFunc); + + m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexConcaveCreateFunc); + m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedConvexConcaveCreateFunc); + + m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_compoundCreateFunc); + + m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedCompoundCreateFunc); + + m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_emptyCreateFunc); + + m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereSphereCF); + +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereBoxCF); + m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_boxSphereCF); +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereTriangleCF); + m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_triangleSphereCF); + m_boxBoxCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_boxBoxCF); + + m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexPlaneCF); + m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_planeConvexCF); + + m_simplexSolver->~btVoronoiSimplexSolver(); + btAlignedFree(m_simplexSolver); + + m_pdSolver->~btConvexPenetrationDepthSolver(); + + btAlignedFree(m_pdSolver); + + +} + + +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +{ + + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } + + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_boxBoxCF; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } + + + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) + { + return m_swappedConvexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return m_emptyCreateFunc; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h new file mode 100644 index 00000000000..df48ee95a18 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -0,0 +1,118 @@ +/* +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_DEFAULT_COLLISION_CONFIGURATION +#define BT_DEFAULT_COLLISION_CONFIGURATION + +#include "btCollisionConfiguration.h" +class btVoronoiSimplexSolver; +class btConvexPenetrationDepthSolver; + +struct btDefaultCollisionConstructionInfo +{ + btStackAlloc* m_stackAlloc; + btPoolAllocator* m_persistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + int m_defaultMaxPersistentManifoldPoolSize; + int m_defaultMaxCollisionAlgorithmPoolSize; + int m_defaultStackAllocatorSize; + + btDefaultCollisionConstructionInfo() + :m_stackAlloc(0), + m_persistentManifoldPool(0), + m_collisionAlgorithmPool(0), + m_defaultMaxPersistentManifoldPoolSize(65535), + m_defaultMaxCollisionAlgorithmPoolSize(65535), + m_defaultStackAllocatorSize(5*1024*1024) + { + } +}; + + + +///btCollisionConfiguration allows to configure Bullet collision detection +///stack allocator, pool memory allocators +///todo: describe the meaning +class btDefaultCollisionConfiguration : public btCollisionConfiguration +{ + +protected: + + int m_persistentManifoldPoolSize; + + btStackAlloc* m_stackAlloc; + bool m_ownsStackAllocator; + + btPoolAllocator* m_persistentManifoldPool; + bool m_ownsPersistentManifoldPool; + + + btPoolAllocator* m_collisionAlgorithmPool; + bool m_ownsCollisionAlgorithmPool; + + //default simplex/penetration depth solvers + btVoronoiSimplexSolver* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + + //default CreationFunctions, filling the m_doubleDispatch table + btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + btCollisionAlgorithmCreateFunc* m_sphereSphereCF; +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + btCollisionAlgorithmCreateFunc* m_sphereBoxCF; + btCollisionAlgorithmCreateFunc* m_boxSphereCF; +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + btCollisionAlgorithmCreateFunc* m_boxBoxCF; + btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; + btCollisionAlgorithmCreateFunc* m_triangleSphereCF; + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; + +public: + + + btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); + + virtual ~btDefaultCollisionConfiguration(); + + ///memory pools + virtual btPoolAllocator* getPersistentManifoldPool() + { + return m_persistentManifoldPool; + } + + virtual btPoolAllocator* getCollisionAlgorithmPool() + { + return m_collisionAlgorithmPool; + } + + virtual btStackAlloc* getStackAllocator() + { + return m_stackAlloc; + } + + + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + + +}; + +#endif //BT_DEFAULT_COLLISION_CONFIGURATION + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index b1a193d2cfd..e54721dec21 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -15,8 +15,9 @@ subject to the following restrictions: #ifndef EMPTY_ALGORITH #define EMPTY_ALGORITH -#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" #define ATTRIBUTE_ALIGNED(a) @@ -33,13 +34,18 @@ public: virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + } + struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { (void)body0; (void)body1; - return new btEmptyAlgorithm(ci); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm)); + return new(mem) btEmptyAlgorithm(ci); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 490acc0b611..d5b12598be2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -79,12 +79,30 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b } btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); - + newPt.m_positionWorldOnA = pointA; + newPt.m_positionWorldOnB = pointInWorld; + int insertIndex = m_manifoldPtr->getCacheEntry(newPt); newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1); newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); + //BP mod, store contact triangles. + newPt.m_partId0 = m_partId0; + newPt.m_partId1 = m_partId1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; + + ///todo, check this for any side effects + if (insertIndex >= 0) + { + //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); + m_manifoldPtr->replaceContactPoint(newPt,insertIndex); + } else + { + insertIndex = m_manifoldPtr->addManifoldPoint(newPt); + } + //User can override friction and/or restitution if (gContactAddedCallback && //and if either of the two bodies requires custom material @@ -94,16 +112,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b //experimental feature info, for per-triangle material etc. btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; - (*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); + (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); } - if (insertIndex >= 0) - { - //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); - m_manifoldPtr->replaceContactPoint(newPt,insertIndex); - } else - { - m_manifoldPtr->AddManifoldPoint(newPt); - } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 77192625513..5aac9a46f6a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -18,12 +18,12 @@ subject to the following restrictions: #define MANIFOLD_RESULT_H class btCollisionObject; -class btPersistentManifold; +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" 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; @@ -60,6 +60,15 @@ public: m_manifoldPtr = manifoldPtr; } + const btPersistentManifold* getPersistentManifold() const + { + return m_manifoldPtr; + } + btPersistentManifold* getPersistentManifold() + { + return m_manifoldPtr; + } + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) { m_partId0=partId0; @@ -70,6 +79,22 @@ public: virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); + SIMD_FORCE_INLINE void refreshContactPoints() + { + btAssert(m_manifoldPtr); + if (!m_manifoldPtr->getNumContacts()) + return; + + bool isSwapped = m_manifoldPtr->getBody0() != m_body0; + + if (isSwapped) + { + m_manifoldPtr->refreshContactPoints(m_rootTransB,m_rootTransA); + } else + { + m_manifoldPtr->refreshContactPoints(m_rootTransA,m_rootTransB); + } + } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index ac2e8554c3a..9f8e27a4407 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -1,3 +1,17 @@ +/* +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 "LinearMath/btScalar.h" @@ -7,7 +21,7 @@ #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include <stdio.h> +//#include <stdio.h> #include "LinearMath/btQuickprof.h" btSimulationIslandManager::btSimulationIslandManager() @@ -25,17 +39,17 @@ void btSimulationIslandManager::initUnionFind(int n) } -void btSimulationIslandManager::findUnions(btDispatcher* dispatcher) +void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) { { - for (int i=0;i<dispatcher->getNumManifolds();i++) - { - const btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); - //static objects (invmass btScalar(0.)) don't merge ! + btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); - const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++) + { + const btBroadphasePair& collisionPair = pairPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && ((colObj1) && ((colObj1)->mergesSimulationIslands()))) @@ -71,7 +85,7 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld } // do the union find - findUnions(dispatcher); + findUnions(dispatcher,colWorld); @@ -91,7 +105,7 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col for (i=0;i<colWorld->getCollisionObjectArray().size();i++) { btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; - if (collisionObject->mergesSimulationIslands()) + if (!collisionObject->isStaticOrKinematicObject()) { collisionObject->setIslandTag( m_unionFind.find(index) ); collisionObject->setCompanionId(-1); @@ -129,30 +143,13 @@ class btPersistentManifoldSortPredicate }; - - - -// -// todo: this is random access, it can be walked 'cache friendly'! -// -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) +void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects) { + BT_PROFILE("islandUnionFindAndQuickSort"); - - /*if (0) - { - int maxNumManifolds = dispatcher->getNumManifolds(); - btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher; - btPersistentManifold** manifold = colDis->getInternalManifoldPointer(); - callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0); - return; - } - */ + m_islandmanifold.resize(0); - - 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 @@ -183,7 +180,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { - printf("error in island management\n"); +// printf("error in island management\n"); } assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); @@ -210,7 +207,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { - printf("error in island management\n"); +// printf("error in island management\n"); } assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); @@ -231,7 +228,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { - printf("error in island management\n"); +// printf("error in island management\n"); } assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); @@ -247,11 +244,17 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, } } - btAlignedObjectArray<btPersistentManifold*> islandmanifold; + int i; int maxNumManifolds = dispatcher->getNumManifolds(); - islandmanifold.reserve(maxNumManifolds); +#define SPLIT_ISLANDS 1 +#ifdef SPLIT_ISLANDS + + +#endif //SPLIT_ISLANDS + + for (i=0;i<maxNumManifolds ;i++) { btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); @@ -265,29 +268,52 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, { //kinematic objects don't merge islands, but wake up all connected objects - if (colObj0->isStaticOrKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) { colObj1->activate(); } - if (colObj1->isStaticOrKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) { colObj0->activate(); } - - //filtering for response +#ifdef SPLIT_ISLANDS + // //filtering for response if (dispatcher->needsResponse(colObj0,colObj1)) - islandmanifold.push_back(manifold); + m_islandmanifold.push_back(manifold); +#endif //SPLIT_ISLANDS } } +} + - int numManifolds = int (islandmanifold.size()); +// +// todo: this is random access, it can be walked 'cache friendly'! +// +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) +{ + + buildIslands(dispatcher,collisionObjects); + + int endIslandIndex=1; + int startIslandIndex; + int numElem = getUnionFind().getNumElements(); + + BT_PROFILE("processIslands"); + +#ifndef SPLIT_ISLANDS + btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); + + callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); +#else // Sort manifolds, based on islands // Sort the vector using predicate and std::sort //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + int numManifolds = int (m_islandmanifold.size()); + //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) - islandmanifold.heapSort(btPersistentManifoldSortPredicate()); + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) @@ -296,10 +322,9 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //int islandId; - END_PROFILE("islandUnionFindAndHeapSort"); - - btAlignedObjectArray<btCollisionObject*> islandBodies; + +// printf("Start Islands\n"); //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) @@ -313,7 +338,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, { int i = getUnionFind().getElement(endIslandIndex).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; - islandBodies.push_back(colObj0); + m_islandBodies.push_back(colObj0); if (!colObj0->isActive()) islandSleeping = true; } @@ -325,12 +350,12 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, if (startManifoldIndex<numManifolds) { - int curIslandId = getIslandId(islandmanifold[startManifoldIndex]); + int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]); if (curIslandId == islandId) { - startManifold = &islandmanifold[startManifoldIndex]; + startManifold = &m_islandmanifold[startManifoldIndex]; - for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++) + for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) { } @@ -342,7 +367,8 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, if (!islandSleeping) { - callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId); + callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); +// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); } if (numIslandManifolds) @@ -350,8 +376,9 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, startManifoldIndex = endManifoldIndex; } - islandBodies.resize(0); + m_islandBodies.resize(0); } +#endif //SPLIT_ISLANDS + - } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index d91ed1c20eb..5f4d54cd803 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -16,18 +16,26 @@ subject to the following restrictions: #ifndef SIMULATION_ISLAND_MANAGER_H #define SIMULATION_ISLAND_MANAGER_H -#include "../CollisionDispatch/btUnionFind.h" +#include "BulletCollision/CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" +#include "LinearMath/btAlignedObjectArray.h" + class btCollisionObject; class btCollisionWorld; class btDispatcher; +class btPersistentManifold; + ///SimulationIslandManager creates and handles simulation islands, using btUnionFind class btSimulationIslandManager { btUnionFind m_unionFind; + btAlignedObjectArray<btPersistentManifold*> m_islandmanifold; + btAlignedObjectArray<btCollisionObject* > m_islandBodies; + + public: btSimulationIslandManager(); virtual ~btSimulationIslandManager(); @@ -42,7 +50,7 @@ public: virtual void storeIslandActivationState(btCollisionWorld* world); - void findUnions(btDispatcher* dispatcher); + void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld); @@ -55,6 +63,8 @@ public: void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); + void buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects); + }; #endif //SIMULATION_ISLAND_MANAGER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 05556bd34e2..1e4bbce451d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -68,18 +68,25 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); + resultOut->setPersistentManifold(m_manifoldPtr); + if (dist < SIMD_EPSILON) { btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize(); /// report a contact. internally this will be kept persistent, and contact reduction is done - resultOut->setPersistentManifold(m_manifoldPtr); resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist); } - + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } } @@ -102,8 +109,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box btVector3 bounds[2]; btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape(); - bounds[0] = -boxShape->getHalfExtents(); - bounds[1] = boxShape->getHalfExtents(); + bounds[0] = -boxShape->getHalfExtentsWithoutMargin(); + bounds[1] = boxShape->getHalfExtentsWithoutMargin(); margins = boxShape->getMargin();//also add sphereShape margin? @@ -209,6 +216,10 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* btVector3 p0, tmp, prel, n[6], normal; btScalar fSep = btScalar(-10000000.0), fSepThis; + // set p0 and normal to a default value to shup up GCC + p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + 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) ); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 07592909200..158f0ffcc74 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -16,11 +16,13 @@ subject to the following restrictions: #ifndef SPHERE_BOX_COLLISION_ALGORITHM_H #define SPHERE_BOX_COLLISION_ALGORITHM_H -#include "../BroadphaseCollision/btCollisionAlgorithm.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" -#include "../CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; -#include "../../LinearMath/btVector3.h" +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. /// Other features are frame-coherency (persistent data) and collision response. @@ -40,6 +42,14 @@ public: virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius ); btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax); @@ -48,12 +58,13 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); if (!m_swapped) { - return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); } else { - return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); } } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 424ff432f84..c2b13f5903f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -46,6 +46,8 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 if (!m_manifoldPtr) return; + resultOut->setPersistentManifold(m_manifoldPtr); + btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape(); btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape(); @@ -54,23 +56,41 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 btScalar radius0 = sphere0->getRadius(); btScalar radius1 = sphere1->getRadius(); +#ifdef CLEAR_MANIFOLD + m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting +#endif + ///iff distance positive, don't generate a new contact if ( len > (radius0+radius1)) + { +#ifndef CLEAR_MANIFOLD + resultOut->refreshContactPoints(); +#endif //CLEAR_MANIFOLD return; - + } ///distance (negative means penetration) btScalar dist = len - (radius0+radius1); - btVector3 normalOnSurfaceB = diff / len; + btVector3 normalOnSurfaceB(1,0,0); + if (len > SIMD_EPSILON) + { + normalOnSurfaceB = diff / len; + } + ///point on A (worldspace) btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; /// report a contact. internally this will be kept persistent, and contact reduction is done - resultOut->setPersistentManifold(m_manifoldPtr); + + resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); +#ifndef CLEAR_MANIFOLD + resultOut->refreshContactPoints(); +#endif //CLEAR_MANIFOLD + } btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 7a19ff31edf..afdc2ee7fa0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -16,9 +16,11 @@ subject to the following restrictions: #ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H #define SPHERE_SPHERE_COLLISION_ALGORITHM_H -#include "../BroadphaseCollision/btCollisionAlgorithm.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" -#include "../CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" + class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. @@ -39,6 +41,13 @@ public: virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } virtual ~btSphereSphereCollisionAlgorithm(); @@ -46,7 +55,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btSphereSphereCollisionAlgorithm(0,ci,body0,body1); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); + return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index b011b707e3f..fdbd4abef27 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -48,8 +48,11 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co if (!m_manifoldPtr) return; - btSphereShape* sphere = (btSphereShape*)col0->getCollisionShape(); - btTriangleShape* triangle = (btTriangleShape*)col1->getCollisionShape(); + btCollisionObject* sphereObj = m_swapped? col1 : col0; + btCollisionObject* triObj = m_swapped? col0 : col1; + + btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape(); + btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape(); /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); @@ -57,11 +60,16 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds - input.m_transformA = col0->getWorldTransform(); - input.m_transformB = col1->getWorldTransform(); + input.m_transformA = sphereObj->getWorldTransform(); + input.m_transformB = triObj->getWorldTransform(); + + bool swapResults = m_swapped; - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); + if (m_ownManifold) + resultOut->refreshContactPoints(); + } btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 57c6e6af619..bbaf228c44c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -16,10 +16,11 @@ subject to the following restrictions: #ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#include "../BroadphaseCollision/btCollisionAlgorithm.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" -#include "../CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; +#include "btCollisionDispatcher.h" /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// Other features are frame-coherency (persistent data) and collision response. @@ -40,6 +41,13 @@ public: virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } virtual ~btSphereTriangleCollisionAlgorithm(); @@ -49,7 +57,9 @@ public: virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); + + return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 62254335796..bb01b60fa85 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -18,6 +18,7 @@ subject to the following restrictions: + btUnionFind::~btUnionFind() { Free(); @@ -76,8 +77,7 @@ void btUnionFind::sortIslands() // Sort the vector using predicate and std::sort //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); - //perhaps use radix sort? - m_elements.heapSort(btUnionFindElementSortPredicate()); + m_elements.quickSort(btUnionFindElementSortPredicate()); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index 236cc33b94f..820c8bc858e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef UNION_FIND_H #define UNION_FIND_H -#include "../../LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAlignedObjectArray.h" #define USE_PATH_COMPRESSION 1 @@ -46,11 +46,11 @@ class btUnionFind void reset(int N); - inline int getNumElements() const + SIMD_FORCE_INLINE int getNumElements() const { return int(m_elements.size()); } - inline bool isRoot(int x) const + SIMD_FORCE_INLINE bool isRoot(int x) const { return (x == m_elements[x].m_id); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index 636b0046c13..8bc2cfa6ffa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -15,35 +15,20 @@ subject to the following restrictions: #include "btBoxShape.h" -btVector3 btBoxShape::getHalfExtents() const -{ - return m_implicitShapeDimensions * m_localScaling; -} + //{ void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - btVector3 halfExtents = getHalfExtents(); - - btMatrix3x3 abs_b = t.getBasis().absolute(); - btPoint3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents), - abs_b[1].dot(halfExtents), - abs_b[2].dot(halfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); - - aabbMin = center - extent; - aabbMax = center + extent; - - + btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); } -void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { //btScalar margin = btScalar(0.); - btVector3 halfExtents = getHalfExtents(); + btVector3 halfExtents = getHalfExtentsWithMargin(); btScalar lx=btScalar(2.)*(halfExtents.x()); btScalar ly=btScalar(2.)*(halfExtents.y()); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index bc42f146c7c..4d4fbefea37 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -18,11 +18,11 @@ subject to the following restrictions: #include "btPolyhedralConvexShape.h" #include "btCollisionMargin.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" -#include "../../LinearMath/btPoint3.h" -#include "../../LinearMath/btSimdMinMax.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "LinearMath/btPoint3.h" +#include "LinearMath/btMinMax.h" -///btBoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box +///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. class btBoxShape: public btPolyhedralConvexShape { @@ -31,47 +31,52 @@ class btBoxShape: public btPolyhedralConvexShape public: - btVector3 getHalfExtents() const; - + btVector3 getHalfExtentsWithMargin() const + { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + btVector3 margin(getMargin(),getMargin(),getMargin()); + halfExtents += margin; + return halfExtents; + } + + const btVector3& getHalfExtentsWithoutMargin() const + { + return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included + } + + virtual int getShapeType() const { return BOX_SHAPE_PROXYTYPE;} virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + btVector3 margin(getMargin(),getMargin(),getMargin()); + halfExtents += margin; - btVector3 halfExtents = getHalfExtents(); - - btVector3 supVertex; - 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; + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - virtual inline btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - btVector3 halfExtents = getHalfExtents(); - btVector3 margin(getMargin(),getMargin(),getMargin()); - halfExtents -= margin; - - 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()); + const btVector3& halfExtents = getHalfExtentsWithoutMargin(); + + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const { - btVector3 halfExtents = getHalfExtents(); - btVector3 margin(getMargin(),getMargin(),getMargin()); - halfExtents -= margin; - - + const btVector3& halfExtents = getHalfExtentsWithoutMargin(); + for (int i=0;i<numVectors;i++) { const btVector3& vec = vectors[i]; - 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()); + supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } } @@ -79,14 +84,38 @@ public: btBoxShape( const btVector3& boxHalfExtents) { - m_implicitShapeDimensions = boxHalfExtents; + btVector3 margin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; }; - + + virtual void setMargin(btScalar collisionMargin) + { + //correct the m_implicitShapeDimensions for the margin + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + + btConvexInternalShape::setMargin(collisionMargin); + btVector3 newMargin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; + + } + virtual void setLocalScaling(const btVector3& scaling) + { + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; + + btConvexInternalShape::setLocalScaling(scaling); + + m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; + + } + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const { @@ -116,7 +145,7 @@ public: virtual void getVertex(int i,btVector3& vtx) const { - btVector3 halfExtents = getHalfExtents(); + btVector3 halfExtents = getHalfExtentsWithoutMargin(); vtx = btVector3( halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), @@ -127,7 +156,7 @@ public: virtual void getPlaneEquation(btVector4& plane,int i) const { - btVector3 halfExtents = getHalfExtents(); + btVector3 halfExtents = getHalfExtentsWithoutMargin(); switch (i) { @@ -234,7 +263,7 @@ public: virtual bool isInside(const btPoint3& pt,btScalar tolerance) const { - btVector3 halfExtents = getHalfExtents(); + btVector3 halfExtents = getHalfExtentsWithoutMargin(); //btScalar minDist = 2*tolerance; @@ -250,7 +279,7 @@ public: //debugging - virtual char* getName()const + virtual const char* getName()const { return "Box"; } @@ -291,3 +320,4 @@ public: #endif //OBB_BOX_MINKOWSKI_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 8da554ef14d..819f6f7f97d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -18,32 +18,56 @@ 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, bool useQuantizedAabbCompression) -:btTriangleMeshShape(meshInterface),m_useQuantizedAabbCompression(useQuantizedAabbCompression) +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh) +:btTriangleMeshShape(meshInterface), +m_bvh(0), +m_useQuantizedAabbCompression(useQuantizedAabbCompression), +m_ownsBvh(false) { //construct bvh from meshInterface #ifndef DISABLE_BVH - m_bvh = new btOptimizedBvh(); btVector3 bvhAabbMin,bvhAabbMax; - meshInterface->calculateAabbBruteForce(bvhAabbMin,bvhAabbMax); - m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + if(meshInterface->hasPremadeAabb()) + { + meshInterface->getPremadeAabb(&bvhAabbMin, &bvhAabbMax); + } + else + { + meshInterface->calculateAabbBruteForce(bvhAabbMin,bvhAabbMax); + } + + if (buildBvh) + { + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); + m_bvh = new (mem) btOptimizedBvh(); + m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + m_ownsBvh = true; + } #endif //DISABLE_BVH } -btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) -:btTriangleMeshShape(meshInterface),m_useQuantizedAabbCompression(useQuantizedAabbCompression) +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh) +:btTriangleMeshShape(meshInterface), +m_bvh(0), +m_useQuantizedAabbCompression(useQuantizedAabbCompression), +m_ownsBvh(false) { //construct bvh from meshInterface #ifndef DISABLE_BVH - m_bvh = new btOptimizedBvh(); - m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + if (buildBvh) + { + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); + m_bvh = new (mem) btOptimizedBvh(); + + m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + m_ownsBvh = true; + } #endif //DISABLE_BVH @@ -58,16 +82,140 @@ void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btV } -void btBvhTriangleMeshShape::refitTree() +void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax) { - m_bvh->refit( m_meshInterface ); + m_bvh->refit( m_meshInterface, aabbMin,aabbMax ); recalcLocalAabb(); } btBvhTriangleMeshShape::~btBvhTriangleMeshShape() { - delete m_bvh; + if (m_ownsBvh) + { + m_bvh->~btOptimizedBvh(); + btAlignedFree(m_bvh); + } +} + +void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) +{ + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btStridingMeshInterface* m_meshInterface; + btTriangleCallback* m_callback; + + MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + nodeSubPart); + + unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j=2;j>=0;j--) + { + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + + /* Perform ray vs. triangle collision here */ + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + } + }; + + MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + + m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); +} + +void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) +{ + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btStridingMeshInterface* m_meshInterface; + btTriangleCallback* m_callback; + + MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + nodeSubPart); + + unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j=2;j>=0;j--) + { + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + + /* Perform ray vs. triangle collision here */ + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + } + }; + + MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + + m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); } //perform bvh tree traversal and report overlapping triangles to 'callback' @@ -118,13 +266,14 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co indicestype, nodeSubPart); - int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j=2;j>=0;j--) { - int graphicsindex = gfxbase[j]; + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; #ifdef DEBUG_TRIANGLE_MESH @@ -157,17 +306,37 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co } - -void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - 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(); - //rebuild the bvh... - m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax); + if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + if (m_ownsBvh) + { + m_bvh->~btOptimizedBvh(); + btAlignedFree(m_bvh); + } + ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); + m_bvh = new(mem) btOptimizedBvh(); + //rebuild the bvh... + m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax); + m_ownsBvh = true; + } +} - } +void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling) +{ + btAssert(!m_bvh); + btAssert(!m_ownsBvh); + + m_bvh = bvh; + m_ownsBvh = false; + // update the scaling without rebuilding the bvh + if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + } } + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 4914d9f959c..6e587340dc1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -18,45 +18,55 @@ subject to the following restrictions: #include "btTriangleMeshShape.h" #include "btOptimizedBvh.h" +#include "LinearMath/btAlignedAllocator.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. + +///The btBvhTriangleMeshShape is a static-triangle mesh shape with several optimizations, such as bounding volume hierarchy and cache friendly traversal for PlayStation 3 Cell SPU. It is recommended to enable useQuantizedAabbCompression for better memory usage. +///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method. +///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk. +///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example. ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape { btOptimizedBvh* m_bvh; bool m_useQuantizedAabbCompression; - bool m_pad[12];////need padding due to alignment + bool m_ownsBvh; + bool m_pad[11];////need padding due to alignment public: - btBvhTriangleMeshShape() :btTriangleMeshShape(0) {}; - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression); + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btBvhTriangleMeshShape() :btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {}; + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); ///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); + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true); virtual ~btBvhTriangleMeshShape(); - - /* + bool getOwnsBvh () const + { + return m_ownsBvh; + } + virtual int getShapeType() const { return TRIANGLE_MESH_SHAPE_PROXYTYPE; } - */ - - + + void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget); + void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - void refitTree(); + void refitTree(const btVector3& aabbMin,const btVector3& aabbMax); ///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";} + virtual const char* getName()const {return "BVHTRIANGLEMESH";} virtual void setLocalScaling(const btVector3& scaling); @@ -65,6 +75,10 @@ public: { return m_bvh; } + + + void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1)); + bool usesQuantizedAabbCompression() const { return m_useQuantizedAabbCompression; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index b7e15172da2..26ba276b900 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -21,6 +21,7 @@ subject to the following restrictions: btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) { + m_upAxis = 1; m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); } @@ -50,7 +51,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) { - btVector3 pos(0,getHalfHeight(),0); + btVector3 pos(0,0,0); + pos[getUpAxis()] = getHalfHeight(); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) @@ -60,7 +63,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) } } { - btVector3 pos(0,-getHalfHeight(),0); + btVector3 pos(0,0,0); + pos[getUpAxis()] = -getHalfHeight(); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) @@ -88,7 +93,8 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) btVector3 vtx; btScalar newDot; { - btVector3 pos(0,getHalfHeight(),0); + btVector3 pos(0,0,0); + pos[getUpAxis()] = getHalfHeight(); vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) @@ -98,7 +104,8 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) } } { - btVector3 pos(0,-getHalfHeight(),0); + btVector3 pos(0,0,0); + pos[getUpAxis()] = -getHalfHeight(); vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) @@ -112,7 +119,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) } -void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { //as an approximation, take the inertia of the box that bounds the spheres @@ -122,7 +129,8 @@ void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) btScalar radius = getRadius(); - btVector3 halfExtents(radius,radius+getHalfHeight(),radius); + btVector3 halfExtents(radius,radius,radius); + halfExtents[getUpAxis()]+=getHalfHeight(); btScalar margin = CONVEX_DISTANCE_MARGIN; @@ -140,6 +148,22 @@ void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) } +btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) +{ + m_upAxis = 0; + m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); +} + + + + + + +btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) +{ + m_upAxis = 2; + m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h index 27da8adefa5..d4b046d40c9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -16,20 +16,27 @@ subject to the following restrictions: #ifndef BT_CAPSULE_SHAPE_H #define BT_CAPSULE_SHAPE_H -#include "btConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "btConvexInternalShape.h" +#include "BulletCollision/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 +///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis. +///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. +///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres. +class btCapsuleShape : public btConvexInternalShape { +protected: + int m_upAxis; + +protected: + ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. + btCapsuleShape() {}; public: btCapsuleShape(btScalar radius,btScalar height); ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; /// btConvexShape Interface virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; @@ -38,21 +45,74 @@ public: virtual int getShapeType() const { return CAPSULE_SHAPE_PROXYTYPE; } - virtual char* getName()const + virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + btVector3 halfExtents(getRadius(),getRadius(),getRadius()); + halfExtents[m_upAxis] = getRadius() + getHalfHeight(); + halfExtents += btVector3(getMargin(),getMargin(),getMargin()); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btPoint3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + + aabbMin = center - extent; + aabbMax = center + extent; + } + + virtual const char* getName()const { return "CapsuleShape"; } + int getUpAxis() const + { + return m_upAxis; + } + btScalar getRadius() const { - return m_implicitShapeDimensions.getX(); + int radiusAxis = (m_upAxis+2)%3; + return m_implicitShapeDimensions[radiusAxis]; } btScalar getHalfHeight() const { - return m_implicitShapeDimensions.getY(); + return m_implicitShapeDimensions[m_upAxis]; + } + +}; + +///btCapsuleShapeX represents a capsule around the Z axis +///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. +class btCapsuleShapeX : public btCapsuleShape +{ +public: + + btCapsuleShapeX(btScalar radius,btScalar height); + + //debugging + virtual const char* getName()const + { + return "CapsuleX"; } + + +}; + +///btCapsuleShapeZ represents a capsule around the Z axis +///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. +class btCapsuleShapeZ : public btCapsuleShape +{ +public: + btCapsuleShapeZ(btScalar radius,btScalar height); + + //debugging + virtual const char* getName()const + { + return "CapsuleZ"; + } + + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 81d82428f4c..39e621aa946 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -21,7 +21,12 @@ subject to the following restrictions: can be used by probes that are checking whether the library is actually installed. */ -extern "C" void btBulletCollisionProbe () {} +extern "C" +{ +void btBulletCollisionProbe (); + +void btBulletCollisionProbe () {} +} @@ -46,7 +51,7 @@ btScalar btCollisionShape::getAngularMotionDisc() const return disc; } -void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) +void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const { //start with static aabb getAabb(curTrans,temporalAabbMin,temporalAabbMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index 96268734a83..3a352b90777 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -16,18 +16,21 @@ 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 "../BroadphaseCollision/btBroadphaseProxy.h" //for the shape types +#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 -///btCollisionShape provides interface for collision shapes that can be shared among btCollisionObjects. +///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. class btCollisionShape { + + void* m_userPointer; + public: - btCollisionShape() + btCollisionShape() : m_userPointer(0) { } virtual ~btCollisionShape() @@ -45,30 +48,30 @@ public: ///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); + void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const; #ifndef __SPU__ - inline bool isPolyhedral() const + SIMD_FORCE_INLINE bool isPolyhedral() const { return btBroadphaseProxy::isPolyhedral(getShapeType()); } - inline bool isConvex() const + SIMD_FORCE_INLINE bool isConvex() const { return btBroadphaseProxy::isConvex(getShapeType()); } - inline bool isConcave() const + SIMD_FORCE_INLINE bool isConcave() const { return btBroadphaseProxy::isConcave(getShapeType()); } - inline bool isCompound() const + SIMD_FORCE_INLINE bool isCompound() const { return btBroadphaseProxy::isCompound(getShapeType()); } ///isInfinite is used to catch simulation error (aabb check) - inline bool isInfinite() const + SIMD_FORCE_INLINE bool isInfinite() const { return btBroadphaseProxy::isInfinite(getShapeType()); } @@ -76,11 +79,11 @@ public: 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; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0; //debugging support - virtual char* getName()const =0 ; + virtual const char* getName()const =0 ; #endif //__SPU__ @@ -88,6 +91,18 @@ public: virtual void setMargin(btScalar margin) = 0; virtual btScalar getMargin() const = 0; + + ///optional user data pointer + void setUserPointer(void* userPtr) + { + m_userPointer = userPtr; + } + + void* getUserPointer() const + { + return m_userPointer; + } + }; #endif //COLLISION_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index a4712b3e925..f08b810eadb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -14,29 +14,42 @@ subject to the following restrictions: */ #include "btCompoundShape.h" - - #include "btCollisionShape.h" - +#include "BulletCollision/BroadphaseCollision/btDbvt.h" btCompoundShape::btCompoundShape() :m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)), -m_aabbTree(0), m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) +m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), +m_dynamicAabbTree(0) { + void* mem = btAlignedAlloc(sizeof(btDbvt),16); + m_dynamicAabbTree = new(mem) btDbvt(); + btAssert(mem==m_dynamicAabbTree); } btCompoundShape::~btCompoundShape() { + if (m_dynamicAabbTree) + { + m_dynamicAabbTree->~btDbvt(); + btAlignedFree(m_dynamicAabbTree); + } } void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) { - m_childTransforms.push_back(localTransform); - m_childShapes.push_back(shape); + //m_childTransforms.push_back(localTransform); + //m_childShapes.push_back(shape); + btCompoundShapeChild child; + child.m_transform = localTransform; + child.m_childShape = shape; + child.m_childShapeType = shape->getShapeType(); + child.m_childMargin = shape->getMargin(); + + m_children.push_back(child); //extend the local aabbMin/aabbMax btVector3 localAabbMin,localAabbMax; @@ -53,39 +66,100 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio } } + if (m_dynamicAabbTree) + { + const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + int index = m_children.size()-1; + child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); + } + +} + +void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) +{ + btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); + if (m_dynamicAabbTree) + { + m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node); + } + m_children.swap(childShapeIndex,m_children.size()-1); + m_children.pop_back(); + +} + +void btCompoundShape::removeChildShape(btCollisionShape* shape) +{ + // Find the children containing the shape specified, and remove those children. + //note: there might be multiple children using the same shape! + for(int i = m_children.size()-1; i >= 0 ; i--) + { + if(m_children[i].m_childShape == shape) + { + m_children.swap(i,m_children.size()-1); + m_children.pop_back(); + //remove it from the m_dynamicAabbTree too + //m_dynamicAabbTree->remove(m_aabbProxies[i]); + //m_aabbProxies.swap(i,m_children.size()-1); + //m_aabbProxies.pop_back(); + } + } + + + + recalculateLocalAabb(); } +void btCompoundShape::recalculateLocalAabb() +{ + // Recalculate the local aabb + // Brute force, it iterates over all the shapes left. + m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + //extend the local aabbMin/aabbMax + for (int j = 0; j < m_children.size(); j++) + { + btVector3 localAabbMin,localAabbMax; + m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); + for (int i=0;i<3;i++) + { + if (m_localAabbMin[i] > localAabbMin[i]) + m_localAabbMin[i] = localAabbMin[i]; + if (m_localAabbMax[i] < localAabbMax[i]) + m_localAabbMax[i] = localAabbMax[i]; + } + } +} - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +///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 = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); 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(getMargin(),getMargin(),getMargin()); + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + aabbMin = center-extent; + aabbMax = center+extent; - aabbMin = center - extent; - aabbMax = center + extent; } -void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { //approximation: take the inertia from the aabb for now btTransform ident; ident.setIdentity(); btVector3 aabbMin,aabbMax; getAabb(ident,aabbMin,aabbMax); - + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - + btScalar lx=btScalar(2.)*(halfExtents.x()); btScalar ly=btScalar(2.)*(halfExtents.y()); btScalar lz=btScalar(2.)*(halfExtents.z()); @@ -96,5 +170,62 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) } - - + + + +void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const +{ + int n = m_children.size(); + + btScalar totalMass = 0; + btVector3 center(0, 0, 0); + for (int k = 0; k < n; k++) + { + center += m_children[k].m_transform.getOrigin() * masses[k]; + totalMass += masses[k]; + } + center /= totalMass; + principal.setOrigin(center); + + btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); + for (int k = 0; k < n; k++) + { + btVector3 i; + m_children[k].m_childShape->calculateLocalInertia(masses[k], i); + + const btTransform& t = m_children[k].m_transform; + btVector3 o = t.getOrigin() - center; + + //compute inertia tensor in coordinate system of compound shape + btMatrix3x3 j = t.getBasis().transpose(); + j[0] *= i[0]; + j[1] *= i[1]; + j[2] *= i[2]; + j = t.getBasis() * j; + + //add inertia tensor + tensor[0] += j[0]; + tensor[1] += j[1]; + tensor[2] += j[2]; + + //compute inertia tensor of pointmass at o + btScalar o2 = o.length2(); + j[0].setValue(o2, 0, 0); + j[1].setValue(0, o2, 0); + j[2].setValue(0, 0, o2); + j[0] += o * -o.x(); + j[1] += o * -o.y(); + j[2] += o * -o.z(); + + //add inertia tensor of pointmass + tensor[0] += masses[k] * j[0]; + tensor[1] += masses[k] * j[1]; + tensor[2] += masses[k] * j[2]; + } + + tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20); + inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); +} + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 86dc1f80947..3624749f768 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -18,58 +18,97 @@ subject to the following restrictions: #include "btCollisionShape.h" -#include "../../LinearMath/btVector3.h" -#include "../../LinearMath/btTransform.h" -#include "../../LinearMath/btMatrix3x3.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" #include "btCollisionMargin.h" -#include "../../LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAlignedObjectArray.h" -class btOptimizedBvh; +//class btOptimizedBvh; +struct btDbvt; + +ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btTransform m_transform; + btCollisionShape* m_childShape; + int m_childShapeType; + btScalar m_childMargin; + struct btDbvtNode* m_node; +}; + +SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2) +{ + return ( c1.m_transform == c2.m_transform && + c1.m_childShape == c2.m_childShape && + c1.m_childShapeType == c2.m_childShapeType && + c1.m_childMargin == c2.m_childMargin ); +} /// btCompoundShape allows to store multiple other btCollisionShapes -/// This allows for concave collision objects. This is more general then the Static Concave btTriangleMeshShape. -class btCompoundShape : public btCollisionShape +/// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape. +ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape { - btAlignedObjectArray<btTransform> m_childTransforms; - btAlignedObjectArray<btCollisionShape*> m_childShapes; + //btAlignedObjectArray<btTransform> m_childTransforms; + //btAlignedObjectArray<btCollisionShape*> m_childShapes; + btAlignedObjectArray<btCompoundShapeChild> m_children; btVector3 m_localAabbMin; btVector3 m_localAabbMax; - btOptimizedBvh* m_aabbTree; + //btOptimizedBvh* m_aabbTree; + btDbvt* m_dynamicAabbTree; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btCompoundShape(); virtual ~btCompoundShape(); void addChildShape(const btTransform& localTransform,btCollisionShape* shape); + /// Remove all children shapes that contain the specified shape + virtual void removeChildShape(btCollisionShape* shape); + + void removeChildShapeByIndex(int childShapeindex); + + int getNumChildShapes() const { - return int (m_childShapes.size()); + return int (m_children.size()); } btCollisionShape* getChildShape(int index) { - return m_childShapes[index]; + return m_children[index].m_childShape; } const btCollisionShape* getChildShape(int index) const { - return m_childShapes[index]; + return m_children[index].m_childShape; } - btTransform& getChildTransform(int index) + btTransform getChildTransform(int index) + { + return m_children[index].m_transform; + } + const btTransform getChildTransform(int index) const { - return m_childTransforms[index]; + return m_children[index].m_transform; } - const btTransform& getChildTransform(int index) const + + + btCompoundShapeChild* getChildList() { - return m_childTransforms[index]; + return &m_children[0]; } ///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; + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. + Use this yourself if you modify the children or their transforms. */ + virtual void recalculateLocalAabb(); virtual void setLocalScaling(const btVector3& scaling) { @@ -80,8 +119,8 @@ public: return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); - + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual int getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} virtual void setMargin(btScalar margin) @@ -92,7 +131,7 @@ public: { return m_collisionMargin; } - virtual char* getName()const + virtual const char* getName()const { return "Compound"; } @@ -100,11 +139,19 @@ public: //this is optional, but should make collision queries faster, by culling non-overlapping nodes void createAabbTreeFromChildren(); - const btOptimizedBvh* getAabbTree() const + btDbvt* getDynamicAabbTree() { - return m_aabbTree; + return m_dynamicAabbTree; } + ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia + ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform + ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound + ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform + ///of the collision object by the principal transform. + void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const; + + private: btScalar m_collisionMargin; protected: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index 73f974e4ee9..f3f57206ab7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -17,12 +17,12 @@ subject to the following restrictions: #define CONCAVE_SHAPE_H #include "btCollisionShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "btTriangleCallback.h" -///Concave shape proves an interface concave shapes that can produce triangles that overlapping a given AABB. -///Static triangle mesh, infinite plane, height field/landscapes are example that implement this interface. +///The btConcaveShape class provides an interface for non-moving (static) concave shapes. +///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape. class btConcaveShape : public btCollisionShape { protected: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 3ccda5b12c6..685369742e4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -16,11 +16,11 @@ subject to the following restrictions: #ifndef CONE_MINKOWSKI_H #define CONE_MINKOWSKI_H -#include "btConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "btConvexInternalShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -///btConeShape implements a Cone shape, around the Y axis -class btConeShape : public btConvexShape +///The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y axis. The btConeShapeX is aligned around the X axis and btConeShapeZ around the Z axis. +class btConeShape : public btConvexInternalShape { @@ -42,7 +42,7 @@ public: btScalar getHeight() const { return m_height;} - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const { btTransform identity; identity.setIdentity(); @@ -72,7 +72,7 @@ public: virtual int getShapeType() const { return CONE_SHAPE_PROXYTYPE; } - virtual char* getName()const + virtual const char* getName()const { return "Cone"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index deb3954b5c3..2596858bc3a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -36,6 +36,13 @@ btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int } + +void btConvexHullShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; + recalcLocalAabb(); +} + void btConvexHullShape::addPoint(const btPoint3& point) { m_points.push_back(point); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 3fd5e382525..4773de2dc51 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -17,18 +17,17 @@ subject to the following restrictions: #define CONVEX_HULL_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include "../../LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/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) +///The btConvexHullShape implements an implicit convex hull of an array of vertices. +///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex. ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape { btAlignedObjectArray<btPoint3> m_points; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); ///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. @@ -43,7 +42,12 @@ public: return &m_points[0]; } - int getNumPoints() + const btPoint3* getPoints() const + { + return &m_points[0]; + } + + int getNumPoints() const { return m_points.size(); } @@ -56,7 +60,7 @@ public: virtual int getShapeType()const { return CONVEX_HULL_SHAPE_PROXYTYPE; } //debugging - virtual char* getName()const {return "Convex";} + virtual const char* getName()const {return "Convex";} virtual int getNumVertices() const; @@ -67,7 +71,8 @@ public: virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const; virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; - + ///in case we receive negative scaling + virtual void setLocalScaling(const btVector3& scaling); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp new file mode 100644 index 00000000000..fb81c8a5bde --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -0,0 +1,78 @@ +/* +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 "btConvexInternalShape.h" + + +btConvexInternalShape::btConvexInternalShape() +: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), +m_collisionMargin(CONVEX_DISTANCE_MARGIN) +{ +} + + +void btConvexInternalShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling.absolute(); +} + + + +void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const +{ + + btScalar margin = getMargin(); + for (int i=0;i<3;i++) + { + 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] = btScalar(-1.); + tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); + minAabb[i] = tmp[i]-margin; + } +}; + + +btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const +{ +#ifndef __SPU__ + + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=btScalar(0.) ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + 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/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h new file mode 100644 index 00000000000..9388ccf31f2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -0,0 +1,98 @@ + +#ifndef BT_CONVEX_INTERNAL_SHAPE_H +#define BT_CONVEX_INTERNAL_SHAPE_H + +#include "btConvexShape.h" + +///The btConvexInternalShape is an internal base class, shared by most convex shape implementations. +class btConvexInternalShape : public btConvexShape +{ + + protected: + + //local scaling. collisionMargin is not scaled ! + btVector3 m_localScaling; + + btVector3 m_implicitShapeDimensions; + + btScalar m_collisionMargin; + + btScalar m_padding; + +public: + + btConvexInternalShape(); + + virtual ~btConvexInternalShape() + { + + } + + + 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 + { + return m_implicitShapeDimensions; + } + + ///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); + } + + + + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const + { + return m_localScaling; + } + + const btVector3& getLocalScalingNV() const + { + return m_localScaling; + } + + virtual void setMargin(btScalar margin) + { + m_collisionMargin = margin; + } + virtual btScalar getMargin() const + { + return m_collisionMargin; + } + + btScalar getMarginNV() const + { + return m_collisionMargin; + } + + virtual int getNumPreferredPenetrationDirections() const + { + return 0; + } + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + (void)penetrationVector; + (void)index; + btAssert(0); + } + + + +}; + + +#endif //BT_CONVEX_INTERNAL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 7edf1ea6db8..7afcccf8b03 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -16,62 +16,3 @@ subject to the following restrictions: #include "btConvexShape.h" -btConvexShape::btConvexShape() -: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), -m_collisionMargin(CONVEX_DISTANCE_MARGIN) -{ -} - - -void btConvexShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; -} - - - -void btConvexShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const -{ - - btScalar margin = getMargin(); - for (int i=0;i<3;i++) - { - 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] = 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()!=btScalar(0.) ) - { - btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - 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 746f383dfc7..e4eb7f6dbf6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -18,37 +18,25 @@ subject to the following restrictions: #include "btCollisionShape.h" -#include "../../LinearMath/btVector3.h" -#include "../../LinearMath/btTransform.h" -#include "../../LinearMath/btMatrix3x3.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" #include "btCollisionMargin.h" +#include "LinearMath/btAlignedAllocator.h" //todo: get rid of this btConvexCastResult thing! struct btConvexCastResult; #define MAX_PREFERRED_PENETRATION_DIRECTIONS 10 -/// 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 +/// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc. +/// It describes general convex shapes using the localGetSupportingVertex interface, used by collision detectors such as btGjkPairDetector. ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape { -protected: - - //local scaling. collisionMargin is not scaled ! - btVector3 m_localScaling; - - btVector3 m_implicitShapeDimensions; - - btScalar m_collisionMargin; - - btScalar m_padding[2]; - - - public: - btConvexShape(); + + BT_DECLARE_ALIGNED_ALLOCATOR(); virtual ~btConvexShape() { @@ -56,7 +44,7 @@ public: } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const =0; #ifndef __SPU__ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; @@ -64,63 +52,24 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; #endif //#ifndef __SPU__ - const btVector3& getImplicitShapeDimensions() const - { - return m_implicitShapeDimensions; - } ///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); - } - - - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const - { - return m_localScaling; - } + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; - const btVector3& getLocalScalingNV() const - { - return m_localScaling; - } + virtual void setLocalScaling(const btVector3& scaling) =0; + virtual const btVector3& getLocalScaling() const =0; - virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; - } - virtual btScalar getMargin() const - { - return m_collisionMargin; - } + virtual void setMargin(btScalar margin)=0; - btScalar getMarginNV() const - { - return m_collisionMargin; - } + virtual btScalar getMargin() const=0; - virtual int getNumPreferredPenetrationDirections() const - { - return 0; - } + virtual int getNumPreferredPenetrationDirections() const=0; - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const - { - (void)penetrationVector; - (void)index; - btAssert(0); - } - - + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0; -} -; +}; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 614ec977793..02a293e82ba 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -19,10 +19,11 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" -btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface) +btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) :m_stridingMesh(meshInterface) { - recalcLocalAabb(); + if ( calcAabb ) + recalcLocalAabb(); } @@ -202,4 +203,114 @@ void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) const btVector3& btConvexTriangleMeshShape::getLocalScaling() const { return m_stridingMesh->getScaling(); -}
\ No newline at end of file +} + +void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const +{ + class CenterCallback: public btInternalTriangleIndexCallback + { + bool first; + btVector3 ref; + btVector3 sum; + btScalar volume; + + public: + + CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void) triangleIndex; + (void) partId; + if (first) + { + ref = triangle[0]; + first = false; + } + else + { + btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); + sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); + volume += vol; + } + } + + btVector3 getCenter() + { + return (volume > 0) ? sum / volume : ref; + } + + btScalar getVolume() + { + return volume * btScalar(1. / 6); + } + + }; + + class InertiaCallback: public btInternalTriangleIndexCallback + { + btMatrix3x3 sum; + btVector3 center; + + public: + + InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void) triangleIndex; + (void) partId; + btMatrix3x3 i; + btVector3 a = triangle[0] - center; + btVector3 b = triangle[1] - center; + btVector3 c = triangle[2] - center; + btVector3 abc = a + b + c; + btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); + for (int j = 0; j < 3; j++) + { + for (int k = 0; k <= j; k++) + { + i[j][k] = i[k][j] = volNeg * (center[j] * center[k] + + btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j]) + + btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); + } + } + btScalar i00 = -i[0][0]; + btScalar i11 = -i[1][1]; + btScalar i22 = -i[2][2]; + i[0][0] = i11 + i22; + i[1][1] = i22 + i00; + i[2][2] = i00 + i11; + sum[0] += i[0]; + sum[1] += i[1]; + sum[2] += i[2]; + } + + btMatrix3x3& getInertia() + { + return sum; + } + + }; + + CenterCallback centerCallback; + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); + btVector3 center = centerCallback.getCenter(); + principal.setOrigin(center); + volume = centerCallback.getVolume(); + + InertiaCallback inertiaCallback(center); + m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); + + btMatrix3x3& i = inertiaCallback.getInertia(); + i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); + inertia.setValue(i[0][0], i[1][1], i[2][2]); + inertia /= volume; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index 34ee7af744c..6ff0bf5d43b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -3,20 +3,24 @@ #include "btPolyhedralConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -/// 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. +/// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape. +/// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead. class btConvexTriangleMeshShape : public btPolyhedralConvexShape { class btStridingMeshInterface* m_stridingMesh; public: - btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface); + btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true); - class btStridingMeshInterface* getStridingMesh() + class btStridingMeshInterface* getMeshInterface() + { + return m_stridingMesh; + } + const class btStridingMeshInterface* getMeshInterface() const { return m_stridingMesh; } @@ -28,7 +32,7 @@ public: virtual int getShapeType()const { return CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; } //debugging - virtual char* getName()const {return "ConvexTrimesh";} + virtual const char* getName()const {return "ConvexTrimesh";} virtual int getNumVertices() const; virtual int getNumEdges() const; @@ -42,6 +46,13 @@ public: virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia + ///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia" + ///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the + ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform + ///of the collision object by the principal transform. This method also computes the volume of the convex mesh. + void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const; + }; @@ -49,3 +60,4 @@ public: #endif //CONVEX_TRIANGLEMESH_SHAPE_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 1666afb3b88..3afef1c7550 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -45,7 +45,7 @@ void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& } -inline btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) +SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) { const int cylinderUpAxis = 0; const int XX = 1; @@ -163,24 +163,24 @@ const int ZZ = 1; btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - return CylinderLocalSupportX(getHalfExtents(),vec); + return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec); } btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - return CylinderLocalSupportZ(getHalfExtents(),vec); + return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec); } btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - return CylinderLocalSupportY(getHalfExtents(),vec); + return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec); } void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtents(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]); } } @@ -188,7 +188,7 @@ void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const b { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtents(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]); } } @@ -199,7 +199,7 @@ void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const b { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtents(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h index c78890c887a..30938acf7c7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -17,10 +17,10 @@ subject to the following restrictions: #define CYLINDER_MINKOWSKI_H #include "btBoxShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include "../../LinearMath/btVector3.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "LinearMath/btVector3.h" -/// implements cylinder shape interface +/// The btCylinderShape class implements a cylinder shape primitive, centered around the origin. Its central axis aligned with the Y axis. btCylinderShapeX is aligned with the X axis and btCylinderShapeZ around the Z axis. class btCylinderShape : public btBoxShape { @@ -60,7 +60,7 @@ public: //use box inertia - // virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + // virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual int getShapeType() const { @@ -74,11 +74,11 @@ public: virtual btScalar getRadius() const { - return getHalfExtents().getX(); + return getHalfExtentsWithMargin().getX(); } //debugging - virtual char* getName()const + virtual const char* getName()const { return "CylinderY"; } @@ -96,14 +96,14 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; //debugging - virtual char* getName()const + virtual const char* getName()const { return "CylinderX"; } virtual btScalar getRadius() const { - return getHalfExtents().getY(); + return getHalfExtentsWithMargin().getY(); } }; @@ -121,14 +121,14 @@ public: return 2; } //debugging - virtual char* getName()const + virtual const char* getName()const { return "CylinderZ"; } virtual btScalar getRadius() const { - return getHalfExtents().getX(); + return getHalfExtentsWithMargin().getX(); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index dcc486f194b..6954aad7917 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -40,7 +40,7 @@ void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aa } -void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) +void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const { btAssert(0); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h index 8ec53444976..d7d78407b61 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -18,16 +18,16 @@ subject to the following restrictions: #include "btConcaveShape.h" -#include "../../LinearMath/btVector3.h" -#include "../../LinearMath/btTransform.h" -#include "../../LinearMath/btMatrix3x3.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" #include "btCollisionMargin.h" -/// btEmptyShape is a collision shape without actual collision detection. -///It can be replaced by another shape during runtime +/// The btEmptyShape is a collision shape without actual collision detection shape, so most users should ignore this class. +/// It can be replaced by another shape during runtime, but the inertia tensor should be recomputed. class btEmptyShape : public btConcaveShape { public: @@ -49,12 +49,12 @@ public: return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual int getShapeType() const { return EMPTY_SHAPE_PROXYTYPE;} - virtual char* getName()const + virtual const char* getName()const { return "Empty"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 94be43c88e2..a291d6b7ce7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -18,16 +18,18 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int width,int length,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) -:m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), -m_width(width), -m_length(length), -m_heightfieldDataUnknown(heightfieldData), +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +: m_heightStickWidth(heightStickWidth), +m_heightStickLength(heightStickLength), m_maxHeight(maxHeight), -m_upAxis(upAxis), +m_width((btScalar)heightStickWidth-1), +m_length((btScalar)heightStickLength-1), +m_heightfieldDataUnknown(heightfieldData), m_useFloatData(useFloatData), m_flipQuadEdges(flipQuadEdges), -m_useDiamondSubdivision(false) +m_useDiamondSubdivision(false), +m_upAxis(upAxis), +m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) { @@ -43,25 +45,25 @@ m_useDiamondSubdivision(false) case 0: { halfExtents.setValue( - m_maxHeight, - m_width, - m_length); + btScalar(m_maxHeight), + btScalar(m_width), //?? don't know if this should change + btScalar(m_length)); break; } case 1: { halfExtents.setValue( - m_width, - m_maxHeight, - m_length); + btScalar(m_width), + btScalar(m_maxHeight), + btScalar(m_length)); break; }; case 2: { halfExtents.setValue( - m_width, - m_length, - m_maxHeight + btScalar(m_width), + btScalar(m_length), + btScalar(m_maxHeight) ); break; } @@ -89,19 +91,15 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape() void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { -/* - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); -*/ - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + halfExtents += btVector3(getMargin(),getMargin(),getMargin()); btMatrix3x3 abs_b = t.getBasis().absolute(); btPoint3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents), abs_b[1].dot(halfExtents), abs_b[2].dot(halfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); + aabbMin = center - extent; aabbMax = center + extent; @@ -114,11 +112,11 @@ btScalar btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const btScalar val = 0.f; if (m_useFloatData) { - val = m_heightfieldDataFloat[(y*m_width)+x]; + val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; } else { //assume unsigned short int - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_width)+x]; + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; val = heightFieldValue* (m_maxHeight/btScalar(65535)); } return val; @@ -133,8 +131,8 @@ void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const btAssert(x>=0); btAssert(y>=0); - btAssert(x<m_width); - btAssert(y<m_length); + btAssert(x<m_heightStickWidth); + btAssert(y<m_heightStickLength); btScalar height = getHeightFieldValue(x,y); @@ -145,25 +143,25 @@ void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const { vertex.setValue( height, - (-m_width/2 ) + x, - (-m_length/2 ) + y + (-m_width/btScalar(2.0)) + x, + (-m_length/btScalar(2.0) ) + y ); break; } case 1: { vertex.setValue( - (-m_width/2 ) + x, + (-m_width/btScalar(2.0)) + x, height, - (-m_length/2 ) + y + (-m_length/btScalar(2.0)) + y ); break; }; case 2: { vertex.setValue( - (-m_width/2 ) + x, - (-m_length/2 ) + y, + (-m_width/btScalar(2.0)) + x, + (-m_length/btScalar(2.0)) + y, height ); break; @@ -180,21 +178,20 @@ void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const } -void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point) const +void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const { - - btVector3 clampedPoint(point); clampedPoint.setMax(m_localAabbMin); clampedPoint.setMin(m_localAabbMax); - btVector3 v = (clampedPoint );// * m_quantization; - - out[0] = (int)(v.getX()); - out[1] = (int)(v.getY()); - out[2] = (int)(v.getZ()); - //correct for + btVector3 v = (clampedPoint);// - m_bvhAabbMin) * m_bvhQuantization; + //TODO: optimization: check out how to removed this btFabs + + out[0] = (int)(v.getX() + v.getX() / btFabs(v.getX())* btScalar(0.5) ); + out[1] = (int)(v.getY() + v.getY() / btFabs(v.getY())* btScalar(0.5) ); + out[2] = (int)(v.getZ() + v.getZ() / btFabs(v.getZ())* btScalar(0.5) ); + } @@ -212,24 +209,24 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); - quantizeWithClamp(quantizedAabbMin, localAabbMin); - quantizeWithClamp(quantizedAabbMax, localAabbMax); + quantizeWithClamp(quantizedAabbMin, localAabbMin,0); + quantizeWithClamp(quantizedAabbMax, localAabbMax,1); int startX=0; - int endX=m_width-1; + int endX=m_heightStickWidth-1; int startJ=0; - int endJ=m_length-1; + int endJ=m_heightStickLength-1; switch (m_upAxis) { case 0: { - quantizedAabbMin[1]+=m_width/2-1; - quantizedAabbMax[1]+=m_width/2+1; - quantizedAabbMin[2]+=m_length/2-1; - quantizedAabbMax[2]+=m_length/2+1; + quantizedAabbMin[1]+=m_heightStickWidth/2-1; + quantizedAabbMax[1]+=m_heightStickWidth/2+1; + quantizedAabbMin[2]+=m_heightStickLength/2-1; + quantizedAabbMax[2]+=m_heightStickLength/2+1; if (quantizedAabbMin[1]>startX) startX = quantizedAabbMin[1]; @@ -243,10 +240,10 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } case 1: { - quantizedAabbMin[0]+=m_width/2-1; - quantizedAabbMax[0]+=m_width/2+1; - quantizedAabbMin[2]+=m_length/2-1; - quantizedAabbMax[2]+=m_length/2+1; + quantizedAabbMin[0]+=m_heightStickWidth/2-1; + quantizedAabbMax[0]+=m_heightStickWidth/2+1; + quantizedAabbMin[2]+=m_heightStickLength/2-1; + quantizedAabbMax[2]+=m_heightStickLength/2+1; if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; @@ -260,10 +257,10 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback }; case 2: { - quantizedAabbMin[0]+=m_width/2-1; - quantizedAabbMax[0]+=m_width/2+1; - quantizedAabbMin[1]+=m_length/2-1; - quantizedAabbMax[1]+=m_length/2+1; + quantizedAabbMin[0]+=m_heightStickWidth/2-1; + quantizedAabbMax[0]+=m_heightStickWidth/2+1; + quantizedAabbMin[1]+=m_heightStickLength/2-1; + quantizedAabbMax[1]+=m_heightStickLength/2+1; if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; @@ -290,7 +287,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback for(int x=startX; x<endX; x++) { btVector3 vertices[3]; - if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j+x) & 1))) + if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))) { //first triangle getVertex(x,j,vertices[0]); @@ -322,7 +319,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const { //moving concave objects not supported diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 49f3e106733..d6890fe6b97 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -18,7 +18,8 @@ subject to the following restrictions: #include "btConcaveShape.h" -///btHeightfieldTerrainShape simulates a 2D heightfield terrain +///The btHeightfieldTerrainShape simulates a 2D heightfield terrain collision shape. You can also use the more general btBvhTriangleMeshShape instead. +///An example implementation of btHeightfieldTerrainShape is provided in Demos/VehicleDemo/VehicleDemo.cpp class btHeightfieldTerrainShape : public btConcaveShape { protected: @@ -26,9 +27,11 @@ protected: btVector3 m_localAabbMax; ///terrain data - int m_width; - int m_length; + int m_heightStickWidth; + int m_heightStickLength; btScalar m_maxHeight; + btScalar m_width; + btScalar m_length; union { unsigned char* m_heightfieldDataUnsignedChar; @@ -45,7 +48,7 @@ protected: btVector3 m_localScaling; virtual btScalar getHeightFieldValue(int x,int y) const; - void quantizeWithClamp(int* out, const btVector3& point) const; + void quantizeWithClamp(int* out, const btVector3& point,int isMax) const; void getVertex(int x,int y,btVector3& vertex) const; inline bool testQuantizedAabbAgainstQuantizedAabb(int* aabbMin1, int* aabbMax1,const int* aabbMin2,const int* aabbMax2) const @@ -58,7 +61,7 @@ protected: } public: - btHeightfieldTerrainShape(int width,int height,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth,int heightStickHeight,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); @@ -74,14 +77,14 @@ public: virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; //debugging - virtual char* getName()const {return "HEIGHTFIELD";} + virtual const char* getName()const {return "HEIGHTFIELD";} }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h new file mode 100644 index 00000000000..7cb6d5ab6fe --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h @@ -0,0 +1,34 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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. +*/ + +/// This file was created by Alex Silverman + +#ifndef MATERIAL_H +#define MATERIAL_H + +// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties +class btMaterial +{ + // public members so that materials can change due to world events +public: + btScalar m_friction; + btScalar m_restitution; + int pad[2]; + + btMaterial(){} + btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } +}; + +#endif // MATERIAL_H
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 015314bc09f..8f893ac98aa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -26,9 +26,9 @@ m_shapeB(shapeB) btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); + btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis())); btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis())); - return supVertexA + supVertexB; + return supVertexA - supVertexB; } void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const @@ -49,7 +49,7 @@ btScalar btMinkowskiSumShape::getMargin() const } -void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { (void)mass; btAssert(0); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index 198faaff9f9..f9c8427c2f3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -16,11 +16,11 @@ subject to the following restrictions: #ifndef MINKOWSKI_SUM_SHAPE_H #define MINKOWSKI_SUM_SHAPE_H -#include "btConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "btConvexInternalShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -/// btMinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. -class btMinkowskiSumShape : public btConvexShape +/// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes. +class btMinkowskiSumShape : public btConvexInternalShape { btTransform m_transA; @@ -37,7 +37,7 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; void setTransformA(const btTransform& transA) { m_transA = transA;} void setTransformB(const btTransform& transB) { m_transB = transB;} @@ -53,7 +53,7 @@ public: const btConvexShape* getShapeA() const { return m_shapeA;} const btConvexShape* getShapeB() const { return m_shapeB;} - virtual char* getName()const + virtual const char* getName()const { return "MinkowskiSum"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index 6015a618082..c709205efc2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -47,7 +47,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) + if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON)) { vec.setValue(1,0,0); } else @@ -116,7 +116,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons -void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { //as an approximation, take the inertia of the box that bounds the spheres diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 1897b474057..45968f3947f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -16,13 +16,14 @@ subject to the following restrictions: #ifndef MULTI_SPHERE_MINKOWSKI_H #define MULTI_SPHERE_MINKOWSKI_H -#include "btConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "btConvexInternalShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #define MAX_NUM_SPHERES 5 -///btMultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) -class btMultiSphereShape : public btConvexShape +///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes. +///It is possible to animate the spheres for deformation. +class btMultiSphereShape : public btConvexInternalShape { @@ -39,7 +40,7 @@ public: btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres); ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; /// btConvexShape Interface virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; @@ -63,7 +64,7 @@ public: virtual int getShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; } - virtual char* getName()const + virtual const char* getName()const { return "MultiSphere"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp new file mode 100644 index 00000000000..fc47e86411d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -0,0 +1,45 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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. +*/ + +/// This file was created by Alex Silverman + +#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" +//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" + + +///Obtains the material for a specific triangle +const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) +{ + const unsigned char * materialBase = 0; + int numMaterials; + PHY_ScalarType materialType; + int materialStride; + const unsigned char * triangleMaterialBase = 0; + int numTriangles; + int triangleMaterialStride; + PHY_ScalarType triangleType; + + ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, + &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); + + // return the pointer to the place with the friction for the triangle + // TODO: This depends on whether it's a moving mesh or not + // BUG IN GIMPACT + //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); + int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); + btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); + return (matVal); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h new file mode 100644 index 00000000000..2f108020cdf --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -0,0 +1,124 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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. +*/ + +/// This file was created by Alex Silverman + +#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H + +#include "btBvhTriangleMeshShape.h" +#include "btMaterial.h" + +///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution. +ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape +{ + btAlignedObjectArray <btMaterial*> m_materialList; + int ** m_triangleMaterials; + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {} + btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): + btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for(int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); + } + } + + ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb + btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true): + btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for(int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); + } + } + + virtual ~btMultimaterialTriangleMeshShape() + { +/* + for(int i = 0; i < m_meshInterface->getNumSubParts(); i++) + { + btAlignedFree(m_materialValues[i]); + m_materialLookup[i] = NULL; + } + btAlignedFree(m_materialValues); + m_materialLookup = NULL; +*/ + } + virtual int getShapeType() const + { + return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + } + + //debugging + virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} + + ///Obtains the material for a specific triangle + const btMaterial * getMaterialProperties(int partID, int triIndex); + +} +; + +#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 44438a24455..a248c55b571 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -19,13 +19,12 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" - -btOptimizedBvh::btOptimizedBvh() : m_useQuantization(false), - m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) - //m_traversalMode(TRAVERSAL_STACKLESS) - //m_traversalMode(TRAVERSAL_RECURSIVE) +btOptimizedBvh::btOptimizedBvh() { +} +btOptimizedBvh::~btOptimizedBvh() +{ } @@ -80,7 +79,7 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback { QuantizedNodeArray& m_triangleNodes; - const btOptimizedBvh* m_optimizedTree; // for quantization + const btQuantizedBvh* m_optimizedTree; // for quantization QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) { @@ -89,14 +88,16 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized return *this; } - QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btOptimizedBvh* tree) + QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree) :m_triangleNodes(triangleNodes),m_optimizedTree(tree) { } virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) { - btAssert(partId==0); + // The partId and triangle index must fit in the same (positive) integer + btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS)); + btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS))); //negative indices are reserved for escapeIndex btAssert(triangleIndex>=0); @@ -111,10 +112,29 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized aabbMin.setMin(triangle[2]); aabbMax.setMax(triangle[2]); - m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMin[0],aabbMin); - m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax); + //PCK: add these checks for zero dimensions of aabb + const btScalar MIN_AABB_DIMENSION = btScalar(0.002); + const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001); + if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION) + { + aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION); + aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION); + } + if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION) + { + aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION); + aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION); + } + if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION) + { + aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION); + aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION); + } - node.m_escapeIndexOrTriangleIndex = triangleIndex; + m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); + m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + + node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_triangleNodes.push_back(node); } @@ -170,10 +190,45 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized subtree.m_rootNodeIndex = 0; subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); } + + //PCK: update the copy of the size + m_subtreeHeaderCount = m_SubtreeHeaders.size(); + + //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary + m_quantizedLeafNodes.clear(); + m_leafNodes.clear(); +} + + + + +void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +{ + if (m_useQuantization) + { + + 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 + { + + } } + void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) { //incrementally initialize quantization values @@ -193,16 +248,17 @@ void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b unsigned short quantizedQueryAabbMin[3]; unsigned short quantizedQueryAabbMax[3]; - quantizeWithClamp(&quantizedQueryAabbMin[0],aabbMin); - quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax); + quantize(&quantizedQueryAabbMin[0],aabbMin,0); + quantize(&quantizedQueryAabbMax[0],aabbMax,1); int i; for (i=0;i<this->m_SubtreeHeaders.size();i++) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); - if (overlap) + //PCK: unsigned instead of bool + unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap != 0) { updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); @@ -212,36 +268,23 @@ void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b } -///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; + int curNodeSubPart=-1; //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); + const unsigned char *vertexbase = 0; + int numverts = 0; + PHY_ScalarType type = PHY_INTEGER; + int stride = 0; + const unsigned char *indexbase = 0; + int indexstride = 0; + int numfaces = 0; + PHY_ScalarType indicestype = PHY_INTEGER; btVector3 triangleVerts[3]; btVector3 aabbMin,aabbMax; @@ -256,16 +299,26 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f if (curNode.isLeafNode()) { //recalc aabb from triangle data + int nodeSubPart = curNode.getPartId(); int nodeTriangleIndex = curNode.getTriangleIndex(); + if (nodeSubPart != curNodeSubPart) + { + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); + + curNodeSubPart = nodeSubPart; + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, - int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); for (int j=2;j>=0;j--) { - int graphicsindex = gfxbase[j]; + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); #ifdef DEBUG_PATCH_COLORS btVector3 mycolor = color[index&3]; @@ -292,8 +345,8 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f aabbMin.setMin(triangleVerts[2]); aabbMax.setMax(triangleVerts[2]); - quantizeWithClamp(&curNode.m_quantizedAabbMin[0],aabbMin); - quantizeWithClamp(&curNode.m_quantizedAabbMax[0],aabbMax); + quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); + quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1); } else { @@ -321,525 +374,17 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f } - 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() -{ -} - -#ifdef DEBUG_TREE_BUILDING -int gStackDepth = 0; -int gMaxStackDepth = 0; -#endif //DEBUG_TREE_BUILDING - -void btOptimizedBvh::buildTree (int startIndex,int endIndex) -{ -#ifdef DEBUG_TREE_BUILDING - gStackDepth++; - if (gStackDepth > gMaxStackDepth) - gMaxStackDepth = gStackDepth; -#endif //DEBUG_TREE_BUILDING - - - int splitAxis, splitIndex, i; - int numIndices =endIndex-startIndex; - int curIndex = m_curNodeIndex; - - assert(numIndices>0); - - if (numIndices==1) - { -#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(startIndex,endIndex); - - splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); - - int internalNodeIndex = m_curNodeIndex; - - 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++) - { - mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i)); - } - - m_curNodeIndex++; - - - //internalNode->m_escapeIndex; - - 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); - -} - -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; - btScalar splitValue; - - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i<endIndex;i++) - { - btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); - means+=center; - } - 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 = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); - if (center[splitAxis] > splitValue) - { - //swap - swapLeafNodes(i,splitIndex); - splitIndex++; - } - } - - //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(int startIndex,int endIndex) -{ - int i; - - 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 = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); - means+=center; - } - means *= (btScalar(1.)/(btScalar)numIndices); - - for (i=startIndex;i<endIndex;i++) - { - btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); - btVector3 diff2 = center-means; - diff2 = diff2 * diff2; - variance += diff2; - } - variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); - - return variance.maxAxis(); -} - - - -void btOptimizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const -{ - //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) - - - 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; - -} + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); -/* -///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); - if (aabbOverlap) - { - isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); - if (isLeafNode) - { - nodeCallback->processNode(rootNode); - } else - { - walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax); - walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax); - } - } - -} -*/ - -void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const -{ - btAssert(m_useQuantization); - - bool aabbOverlap, isLeafNode; - - 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 +///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' +btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) { - btAssert(m_useQuantization); - - int curIndex = startNodeIndex; - int walkIterations = 0; - int subTreeSize = endNodeIndex - startNodeIndex; - - const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; - int escapeIndex; + btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - bool aabbOverlap, isLeafNode; - - 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 < subTreeSize); - - walkIterations++; - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); - isLeafNode = rootNode->isLeafNode(); - - if (isLeafNode && aabbOverlap) - { - nodeCallback->processNode(0,rootNode->getTriangleIndex()); - } - - if (aabbOverlap || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - 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]; - } + //we don't add additional data so just do a static upcast + return static_cast<btOptimizedBvh*>(bvh); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index d5159586344..d7536417d7e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -16,315 +16,48 @@ subject to the following restrictions: #ifndef OPTIMIZED_BVH_H #define OPTIMIZED_BVH_H - -#include "../../LinearMath/btVector3.h" - - -//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp - - +#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" class btStridingMeshInterface; -//Note: currently we have 16 bytes per quantized node -#define MAX_SUBTREE_SIZE_IN_BYTES 2048 - -///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 +///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface. +ATTRIBUTE_ALIGNED16(class) btOptimizedBvh : public btQuantizedBvh { - //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(int subPart, int triangleIndex) = 0; -}; - -#include "../../LinearMath/btAlignedAllocator.h" -#include "../../LinearMath/btAlignedObjectArray.h" - - - -///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) -ATTRIBUTE_ALIGNED16(class) btOptimizedBvh -{ - NodeArray m_leafNodes; - NodeArray m_contiguousNodes; - - QuantizedNodeArray m_quantizedLeafNodes; - - QuantizedNodeArray m_quantizedContiguousNodes; - - int m_curNodeIndex; - - - //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 - }; - - btTraversalMode m_traversalMode; - - - - - 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); + BT_DECLARE_ALIGNED_ALLOCATOR(); protected: - - - void buildTree (int startIndex,int endIndex); - - int calcSplittingAxis(int startIndex,int endIndex); - - int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); - - 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; - - - 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(); 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 refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax); void refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax); void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index); + /// Data buffer MUST be 16 byte aligned + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) + { + return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - QuantizedNodeArray& getQuantizedNodeArray() - { - return m_quantizedContiguousNodes; } - BvhSubtreeInfoArray& getSubtreeInfoArray() - { - return m_SubtreeHeaders; - } + ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' + static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); -} -; + +}; #endif //OPTIMIZED_BVH_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index bbc4ba62af6..71c3af38a02 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#include <BulletCollision/CollisionShapes/btPolyhedralConvexShape.h> +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" btPolyhedralConvexShape::btPolyhedralConvexShape() :m_localAabbMin(1,1,1), @@ -95,7 +95,7 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( -void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { //not yet, return box inertia @@ -128,10 +128,46 @@ void btPolyhedralConvexShape::getAabb(const btTransform& trans,btVector3& aabbMi +void btPolyhedralConvexShape::setLocalScaling(const btVector3& scaling) +{ + btConvexInternalShape::setLocalScaling(scaling); + recalcLocalAabb(); +} void btPolyhedralConvexShape::recalcLocalAabb() { m_isLocalAabbValid = true; + + #if 1 + static const btVector3 _directions[] = + { + btVector3( 1., 0., 0.), + btVector3( 0., 1., 0.), + btVector3( 0., 0., 1.), + btVector3( -1., 0., 0.), + btVector3( 0., -1., 0.), + btVector3( 0., 0., -1.) + }; + + btVector3 _supporting[] = + { + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.) + }; + + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); + + for ( int i = 0; i < 3; ++i ) + { + m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; + m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; + } + + #else for (int i=0;i<3;i++) { @@ -143,6 +179,8 @@ void btPolyhedralConvexShape::recalcLocalAabb() tmp = localGetSupportingVertex(vec); m_localAabbMin[i] = tmp[i]-m_collisionMargin; } + #endif } + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index c35f7512663..4c4ce7feaa7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -16,13 +16,14 @@ subject to the following restrictions: #ifndef BU_SHAPE #define BU_SHAPE -#include "../../LinearMath/btPoint3.h" -#include "../../LinearMath/btMatrix3x3.h" -#include "btConvexShape.h" +#include "LinearMath/btPoint3.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAabbUtil2.h" +#include "btConvexInternalShape.h" -///PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes. -class btPolyhedralConvexShape : public btConvexShape +///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes. +class btPolyhedralConvexShape : public btConvexInternalShape { protected: @@ -38,7 +39,7 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const @@ -46,33 +47,14 @@ public: //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; - - + btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); } virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + void recalcLocalAabb(); virtual int getNumVertices() const = 0 ; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp new file mode 100644 index 00000000000..2b81a02b557 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -0,0 +1,121 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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 "btScaledBvhTriangleMeshShape.h" + +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling) +:m_bvhTriMeshShape(childShape), +m_localScaling(localScaling) +{ + +} + +btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() +{ +} + + +class btScaledTriangleCallback : public btTriangleCallback +{ + btTriangleCallback* m_originalCallback; + + btVector3 m_localScaling; + +public: + + btScaledTriangleCallback(btTriangleCallback* originalCallback,btVector3 localScaling) + :m_originalCallback(originalCallback), + m_localScaling(localScaling) + { + } + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btVector3 newTriangle[3]; + newTriangle[0] = triangle[0]*m_localScaling; + newTriangle[1] = triangle[1]*m_localScaling; + newTriangle[2] = triangle[2]*m_localScaling; + m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); + } +}; + +void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + btScaledTriangleCallback scaledCallback(callback,m_localScaling); + + btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); + btVector3 scaledAabbMin,scaledAabbMax; + + ///support negative scaling + scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; + scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; + scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; + + scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; + scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; + scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; + + + m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); +} + + +void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); + btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); + + btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling; + btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling; + + localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; + localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; + localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; + localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; + localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; + localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; + + btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + btScalar margin = m_bvhTriMeshShape->getMargin(); + localHalfExtents += btVector3(margin,margin,margin); + btVector3 localCenter = btScalar(0.5)*(localAabbMax+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)); + aabbMin = center - extent; + aabbMax = center + extent; + +} + +void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; +} + +const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const +{ + return m_localScaling; +} + +void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + ///don't make this a movable object! +// btAssert(0); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h new file mode 100644 index 00000000000..0a1ef8ce5cb --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -0,0 +1,67 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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 SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" + + +///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. +///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling' +ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape +{ + + + btVector3 m_localScaling; + + btBvhTriangleMeshShape* m_bvhTriMeshShape; + +public: + + + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling); + + virtual ~btScaledBvhTriangleMeshShape(); + + virtual int getShapeType() const + { + //use un-used 'FAST_CONCAVE_MESH_PROXYTYPE' for now, later add SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE to btBroadphaseProxy.h + return SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; + } + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + btBvhTriangleMeshShape* getChildShape() + { + return m_bvhTriMeshShape; + } + + const btBvhTriangleMeshShape* getChildShape() const + { + return m_bvhTriMeshShape; + } + + //debugging + virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + +}; + +#endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp new file mode 100644 index 00000000000..a87b87f1a6f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -0,0 +1,164 @@ +/* +btbtShapeHull implemented by John McCutchan. + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 "btShapeHull.h" +#include "LinearMath/btConvexHull.h" + +#define NUM_UNITSPHERE_POINTS 42 + +static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = +{ + 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)) +}; + +btShapeHull::btShapeHull (const btConvexShape* shape) +{ + m_shape = shape; + m_vertices.clear (); + m_indices.clear(); + m_numIndices = 0; +} + +btShapeHull::~btShapeHull () +{ + m_indices.clear(); + m_vertices.clear (); +} + +bool +btShapeHull::buildHull (btScalar /*margin*/) +{ + int numSampleDirections = NUM_UNITSPHERE_POINTS; + { + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;i<numPDA;i++) + { + btVector3 norm; + m_shape->getPreferredPenetrationDirection(i,norm); + btUnitSpherePoints[numSampleDirections] = norm; + numSampleDirections++; + } + } + } + + btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + int i; + for (i = 0; i < numSampleDirections; i++) + { + supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]); + } + + HullDesc hd; + hd.mFlags = QF_TRIANGLES; + hd.mVcount = static_cast<unsigned int>(numSampleDirections); + +#ifdef BT_USE_DOUBLE_PRECISION + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof(btVector3); +#else + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof (btVector3); +#endif + + HullLibrary hl; + HullResult hr; + if (hl.CreateConvexHull (hd, hr) == QE_FAIL) + { + return false; + } + + m_vertices.resize (static_cast<int>(hr.mNumOutputVertices)); + + + for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++) + { + m_vertices[i] = hr.m_OutputVertices[i]; + } + m_numIndices = hr.mNumIndices; + m_indices.resize(static_cast<int>(m_numIndices)); + for (i = 0; i < static_cast<int>(m_numIndices); i++) + { + m_indices[i] = hr.m_Indices[i]; + } + + // free temporary hull result that we just copied + hl.ReleaseResult (hr); + + return true; +} + +int +btShapeHull::numTriangles () const +{ + return static_cast<int>(m_numIndices / 3); +} + +int +btShapeHull::numVertices () const +{ + return m_vertices.size (); +} + +int +btShapeHull::numIndices () const +{ + return static_cast<int>(m_numIndices); +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h new file mode 100644 index 00000000000..583c6b99eb6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -0,0 +1,56 @@ +/* +btShapeHull implemented by John McCutchan. + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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 _SHAPE_HULL_H +#define _SHAPE_HULL_H + +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" + + +///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices. +///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object. +///It approximates the convex hull using the supporting vertex of 42 directions. +class btShapeHull +{ +public: + btShapeHull (const btConvexShape* shape); + ~btShapeHull (); + + bool buildHull (btScalar margin); + + int numTriangles () const; + int numVertices () const; + int numIndices () const; + + const btVector3* getVertexPointer() const + { + return &m_vertices[0]; + } + const unsigned int* getIndexPointer() const + { + return &m_indices[0]; + } + +protected: + btAlignedObjectArray<btVector3> m_vertices; + btAlignedObjectArray<unsigned int> m_indices; + unsigned int m_numIndices; + const btConvexShape* m_shape; +}; + +#endif //_SHAPE_HULL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp index ca65dd03f3e..15cfe432e27 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -68,7 +68,7 @@ void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& a -void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { 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 66521664087..a6d01cdca9d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -16,15 +16,17 @@ subject to the following restrictions: #ifndef SPHERE_MINKOWSKI_H #define SPHERE_MINKOWSKI_H -#include "btConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "btConvexInternalShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -///btSphereShape implements an implicit (getSupportingVertex) Sphere -ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexShape +///The btSphereShape implements an implicit sphere, centered around a local origin with radius. +ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape { public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btSphereShape (btScalar radius); @@ -34,26 +36,32 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual int getShapeType() const { return SPHERE_SHAPE_PROXYTYPE; } - btScalar getRadius() const { return m_implicitShapeDimensions.getX();} + btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();} + + void setUnscaledRadius(btScalar radius) + { + m_implicitShapeDimensions.setX(radius); + btConvexInternalShape::setMargin(radius); + } //debugging - virtual char* getName()const {return "SPHERE";} + virtual const char* getName()const {return "SPHERE";} virtual void setMargin(btScalar margin) { - btConvexShape::setMargin(margin); + btConvexInternalShape::setMargin(margin); } 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 - return m_localScaling.getX() * getRadius() + btConvexShape::getMargin(); + return getRadius(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index 4fbaafa1b93..3c98ca08ddc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -19,10 +19,11 @@ subject to the following restrictions: btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant) -:m_planeNormal(planeNormal), +:m_planeNormal(planeNormal.normalized()), m_planeConstant(planeConstant), m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.)) { + // btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) ); } @@ -86,7 +87,7 @@ void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const } -void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { (void)mass; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index f59cc0c3347..ddebca73dea 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -19,8 +19,7 @@ subject to the following restrictions: #include "btConcaveShape.h" -///StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB. -///Assumed is that the other objects is not also infinite, so a reasonable sized AABB. +///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane. class btStaticPlaneShape : public btConcaveShape { protected: @@ -46,14 +45,23 @@ public: virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + const btVector3& getPlaneNormal() const + { + return m_planeNormal; + } + + const btScalar& getPlaneConstant() const + { + return m_planeConstant; + } //debugging - virtual char* getName()const {return "STATICPLANE";} + virtual const char* getName()const {return "STATICPLANE";} }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index 03ca1ae7736..752a5fc1dfa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -51,7 +51,7 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde { for (gfxindex=0;gfxindex<numtriangles;gfxindex++) { - int* tri_indices= (int*)(indexbase+gfxindex*indexstride); + unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); @@ -66,7 +66,7 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde { for (gfxindex=0;gfxindex<numtriangles;gfxindex++) { - short int* tri_indices= (short int*)(indexbase+gfxindex*indexstride); + unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); @@ -121,4 +121,4 @@ void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVecto 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 d7b354b7855..85020dd3f58 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. @@ -29,7 +29,8 @@ typedef enum PHY_ScalarType { PHY_FIXEDPOINT88 } PHY_ScalarType; -/// btStridingMeshInterface is the interface class for high performance access to triangle meshes +/// The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape and some other collision shapes. +/// Using index striding of 3*sizeof(integer) it can use triangle arrays, using index striding of 1*sizeof(integer) it can handle triangle strips. /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. class btStridingMeshInterface { @@ -47,7 +48,7 @@ class btStridingMeshInterface - void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; ///brute force method to calculate aabb void calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax); @@ -75,6 +76,10 @@ class btStridingMeshInterface virtual void preallocateVertices(int numverts)=0; virtual void preallocateIndices(int numindices)=0; + virtual bool hasPremadeAabb() const { return false; } + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const {} + virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const {} + const btVector3& getScaling() const { return m_scaling; } @@ -83,6 +88,7 @@ class btStridingMeshInterface m_scaling = scaling; } + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h index 94bc4ec0fa5..09272d61dc2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -18,10 +18,10 @@ subject to the following restrictions: #include "btPolyhedralConvexShape.h" -#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -///BU_Simplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). +///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead. class btBU_Simplex1to4 : public btPolyhedralConvexShape { protected: @@ -68,7 +68,7 @@ public: ///getName is for debugging - virtual char* getName()const { return "btBU_Simplex1to4";} + virtual const char* getName()const { return "btBU_Simplex1to4";} }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp index 54864c32f3a..5194219f256 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp @@ -16,14 +16,7 @@ subject to the following restrictions: #include "btTriangleBuffer.h" -///example usage of this class: -// btTriangleBuffer triBuf; -// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax); -// for (int i=0;i<triBuf.getNumTriangles();i++) -// { -// const btTriangle& tri = triBuf.getTriangle(i); -// //do something useful here with the triangle -// } + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index 3805c519d22..c739fc11e8f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define BT_TRIANGLE_BUFFER_H #include "btTriangleCallback.h" -#include "../../LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAlignedObjectArray.h" struct btTriangle { @@ -29,6 +29,14 @@ struct btTriangle }; ///btTriangleBuffer can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' +///Example usage of this class: +/// btTriangleBuffer triBuf; +/// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax); +/// for (int i=0;i<triBuf.getNumTriangles();i++) +/// { +/// const btTriangle& tri = triBuf.getTriangle(i); +/// //do something useful here with the triangle +/// } class btTriangleBuffer : public btTriangleCallback { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h index fbb87bc4fd8..cb760be80ca 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -16,9 +16,11 @@ subject to the following restrictions: #ifndef TRIANGLE_CALLBACK_H #define TRIANGLE_CALLBACK_H -#include "../../LinearMath/btVector3.h" +#include "LinearMath/btVector3.h" +///The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTriangles. +///This callback is called by processAllTriangles for all btConcaveShape derived class, such as btBvhTriangleMeshShape, btStaticPlaneShape and btHeightfieldTerrainShape. class btTriangleCallback { public: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index 00847861cf1..b962829041a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -4,8 +4,8 @@ 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, +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. @@ -16,36 +16,46 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride) +: m_hasAabb(0) { btIndexedMesh mesh; - + mesh.m_numTriangles = numTriangles; mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase; mesh.m_triangleIndexStride = triangleIndexStride; mesh.m_numVertices = numVertices; mesh.m_vertexBase = (const unsigned char *)vertexBase; mesh.m_vertexStride = vertexStride; - + addIndexedMesh(mesh); } +btTriangleIndexVertexArray::~btTriangleIndexVertexArray() +{ + +} + void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) { btAssert(subpart< getNumSubParts() ); - + btIndexedMesh& mesh = m_indexedMeshes[subpart]; numverts = mesh.m_numVertices; (*vertexbase) = (unsigned char *) mesh.m_vertexBase; + #ifdef BT_USE_DOUBLE_PRECISION + type = PHY_DOUBLE; + #else type = PHY_FLOAT; + #endif vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; - indicestype = PHY_INTEGER; + indicestype = mesh.m_indexType; } void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const @@ -54,12 +64,34 @@ void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned numverts = mesh.m_numVertices; (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; + #ifdef BT_USE_DOUBLE_PRECISION + type = PHY_DOUBLE; + #else type = PHY_FLOAT; + #endif vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; - indicestype = PHY_INTEGER; + indicestype = mesh.m_indexType; +} + +bool btTriangleIndexVertexArray::hasPremadeAabb() const +{ + return (m_hasAabb == 1); +} + +void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) +{ + m_aabbMin = aabbMin; + m_aabbMax = aabbMax; + m_hasAabb = 1; // this is intentionally an int see notes in header +} + +void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const +{ + *aabbMin = m_aabbMin; + *aabbMax = m_aabbMax; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 6ab6a762b39..e546675f802 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -17,48 +17,62 @@ subject to the following restrictions: #define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H #include "btStridingMeshInterface.h" -#include "../../LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btScalar.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 + +///The btIndexedMesh indexes a single vertex and index array. Multiple btIndexedMesh objects can be passed into a btTriangleIndexVertexArray using addIndexedMesh. +///Instead of the number of indices, we pass the number of triangles. ATTRIBUTE_ALIGNED16( struct) btIndexedMesh { + BT_DECLARE_ALIGNED_ALLOCATOR(); + 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]; + // The index type is set when adding an indexed mesh to the + // btTriangleIndexVertexArray, do not set it manually + PHY_ScalarType m_indexType; + int pad; } ; typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray; -///TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. +///The btTriangleIndexVertexArray allows to access multiple triangle 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. ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface { +protected: IndexedMeshArray m_indexedMeshes; - int m_pad[3]; + int m_pad[2]; + int m_hasAabb; // using int instead of bool to maintain alignment + btVector3 m_aabbMin; + btVector3 m_aabbMax; - public: - btTriangleIndexVertexArray() + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btTriangleIndexVertexArray() : m_hasAabb(0) { } + virtual ~btTriangleIndexVertexArray(); + //just to be backwards compatible - btTriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); + btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); - void addIndexedMesh(const btIndexedMesh& mesh) + void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) { m_indexedMeshes.push_back(mesh); + m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType; } @@ -91,6 +105,10 @@ public: virtual void preallocateVertices(int numverts){(void) numverts;} virtual void preallocateIndices(int numindices){(void) numindices;} + virtual bool hasPremadeAabb() const; + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ); + virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const; + } ; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp new file mode 100644 index 00000000000..492854ff646 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -0,0 +1,86 @@ +/* +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. +*/ + +///This file was created by Alex Silverman + +#include "btTriangleIndexVertexMaterialArray.h" + +btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, + int numVertices,btScalar* vertexBase,int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride) : +btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) +{ + btMaterialProperties mat; + + mat.m_numMaterials = numMaterials; + mat.m_materialBase = materialBase; + mat.m_materialStride = materialStride; +#ifdef BT_USE_DOUBLE_PRECISION + mat.m_materialType = PHY_DOUBLE; +#else + mat.m_materialType = PHY_FLOAT; +#endif + + mat.m_numTriangles = numTriangles; + mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase; + mat.m_triangleMaterialStride = materialIndexStride; + mat.m_triangleType = PHY_INTEGER; + + addMaterialProperties(mat); +} + + +void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +{ + btAssert(subpart< getNumSubParts() ); + + btMaterialProperties& mats = m_materials[subpart]; + + numMaterials = mats.m_numMaterials; + (*materialBase) = (unsigned char *) mats.m_materialBase; +#ifdef BT_USE_DOUBLE_PRECISION + materialType = PHY_DOUBLE; +#else + materialType = PHY_FLOAT; +#endif + materialStride = mats.m_materialStride; + + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; +} + +void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +{ + btMaterialProperties& mats = m_materials[subpart]; + + numMaterials = mats.m_numMaterials; + (*materialBase) = (const unsigned char *) mats.m_materialBase; +#ifdef BT_USE_DOUBLE_PRECISION + materialType = PHY_DOUBLE; +#else + materialType = PHY_FLOAT; +#endif + materialStride = mats.m_materialStride; + + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h new file mode 100644 index 00000000000..69a2e631458 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -0,0 +1,84 @@ +/* +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. +*/ + +///This file was created by Alex Silverman + +#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H +#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H + +#include "btTriangleIndexVertexArray.h" + + +ATTRIBUTE_ALIGNED16( struct) btMaterialProperties +{ + ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution + int m_numMaterials; + const unsigned char * m_materialBase; + int m_materialStride; + PHY_ScalarType m_materialType; + ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're + /// padding the structure, it can be reproduced at no real cost + ///m_triangleMaterials =====> 1 integer value makes up one entry + /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 + int m_numTriangles; + const unsigned char * m_triangleMaterialsBase; + int m_triangleMaterialStride; + ///m_triangleType <========== Automatically set in addMaterialProperties + PHY_ScalarType m_triangleType; +}; + +typedef btAlignedObjectArray<btMaterialProperties> MaterialArray; + +///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray +///The addition of a material array allows for the utilization of the partID and +///triangleIndex that are returned in the ContactAddedCallback. As with +///TriangleIndexVertexArray, no duplicate is made of the material data, so it +///is the users responsibility to maintain the array during the lifetime of the +///TriangleIndexVertexMaterialArray. +ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray +{ +protected: + MaterialArray m_materials; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btTriangleIndexVertexMaterialArray() + { + } + + btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, + int numVertices,btScalar* vertexBase,int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride); + + virtual ~btTriangleIndexVertexMaterialArray() {} + + void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) + { + m_materials.push_back(mat); + m_materials[m_materials.size()-1].m_triangleType = triangleType; + } + + virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0); + + virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); + +} +; + +#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 98c54ef45f8..29d26316316 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -14,47 +14,120 @@ subject to the following restrictions: */ #include "btTriangleMesh.h" -#include <assert.h> -btTriangleMesh::btTriangleMesh () + +btTriangleMesh::btTriangleMesh (bool use32bitIndices,bool use4componentVertices) +:m_use32bitIndices(use32bitIndices), +m_use4componentVertices(use4componentVertices), +m_weldingThreshold(0.0) { + btIndexedMesh meshIndex; + meshIndex.m_numTriangles = 0; + meshIndex.m_numVertices = 0; + meshIndex.m_indexType = PHY_INTEGER; + meshIndex.m_triangleIndexBase = 0; + meshIndex.m_triangleIndexStride = 3*sizeof(int); + meshIndex.m_vertexBase = 0; + meshIndex.m_vertexStride = sizeof(btVector3); + m_indexedMeshes.push_back(meshIndex); -} + if (m_use32bitIndices) + { + m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3; + m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0]; + m_indexedMeshes[0].m_indexType = PHY_INTEGER; + m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int); + } else + { + m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3; + m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0]; + m_indexedMeshes[0].m_indexType = PHY_SHORT; + m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int); + } -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) -{ - (void)subpart; - numverts = m_vertices.size(); - *vertexbase = (unsigned char*)&m_vertices[0]; - type = PHY_FLOAT; - stride = sizeof(btVector3); + if (m_use4componentVertices) + { + m_indexedMeshes[0].m_numVertices = m_4componentVertices.size(); + m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0]; + m_indexedMeshes[0].m_vertexStride = sizeof(btVector3); + } else + { + m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3; + m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; + m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar); + } - numfaces = m_indices.size()/3; - *indexbase = (unsigned char*) &m_indices[0]; - indicestype = PHY_INTEGER; - 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 +void btTriangleMesh::addIndex(int index) { - (void)subpart; - numverts = m_vertices.size(); - *vertexbase = (unsigned char*)&m_vertices[0]; - type = PHY_FLOAT; - stride = sizeof(btVector3); - - numfaces = m_indices.size()/3; - *indexbase = (unsigned char*) &m_indices[0]; - indicestype = PHY_INTEGER; - indexstride = 3*sizeof(int); - + if (m_use32bitIndices) + { + m_32bitIndices.push_back(index); + m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0]; + } else + { + m_16bitIndices.push_back(index); + m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0]; + } } +int btTriangleMesh::findOrAddVertex(const btVector3& vertex) +{ + //return index of new/existing vertex + //todo: could use acceleration structure for this + if (m_use4componentVertices) + { + for (int i=0;i< m_4componentVertices.size();i++) + { + if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold) + { + return i; + } + } + m_indexedMeshes[0].m_numVertices++; + m_4componentVertices.push_back(vertex); + m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0]; + + return m_4componentVertices.size()-1; + + } else + { + + for (int i=0;i< m_3componentVertices.size();i+=3) + { + btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]); + if ((vtx-vertex).length2() <= m_weldingThreshold) + { + return i/3; + } + } + m_3componentVertices.push_back(vertex.getX()); + m_3componentVertices.push_back(vertex.getY()); + m_3componentVertices.push_back(vertex.getZ()); + m_indexedMeshes[0].m_numVertices++; + m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; + return (m_3componentVertices.size()/3)-1; + } +} + +void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2) +{ + m_indexedMeshes[0].m_numTriangles++; + + addIndex(findOrAddVertex(vertex0)); + addIndex(findOrAddVertex(vertex1)); + addIndex(findOrAddVertex(vertex2)); +} -int btTriangleMesh::getNumSubParts() const +int btTriangleMesh::getNumTriangles() const { - return 1; + if (m_use32bitIndices) + { + return m_32bitIndices.size() / 3; + } + return m_16bitIndices.size() / 3; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 525f5336b48..1f51b2f2c87 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -17,54 +17,48 @@ subject to the following restrictions: #ifndef TRIANGLE_MESH_H #define TRIANGLE_MESH_H -#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 +#include "btTriangleIndexVertexArray.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +///The btTriangleMesh class is a convenience class derived from btTriangleIndexVertexArray, that provides storage for a concave triangle mesh. It can be used as data for the btBvhTriangleMeshShape. +///It allows either 32bit or 16bit indices, and 4 (x-y-z-w) or 3 (x-y-z) component vertices. +///If you want to share triangle/index data between graphics mesh and collision mesh (btBvhTriangleMeshShape), you can directly use btTriangleIndexVertexArray or derive your own class from btStridingMeshInterface. +///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same. +///It has a brute-force option to weld together closeby vertices. +class btTriangleMesh : public btTriangleIndexVertexArray { - btAlignedObjectArray<btVector3> m_vertices; - btAlignedObjectArray<int> m_indices; + btAlignedObjectArray<btVector3> m_4componentVertices; + btAlignedObjectArray<float> m_3componentVertices; + + btAlignedObjectArray<unsigned int> m_32bitIndices; + btAlignedObjectArray<unsigned short int> m_16bitIndices; + bool m_use32bitIndices; + bool m_use4componentVertices; + public: - btTriangleMesh (); + btScalar m_weldingThreshold; + + btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); - void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2) + int findOrAddVertex(const btVector3& vertex); + void addIndex(int index); + + bool getUse32bitIndices() const { - 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++); + return m_use32bitIndices; } - int getNumTriangles() const + bool getUse4componentVertices() const { - return m_indices.size() / 3; + return m_use4componentVertices; } - - -//StridingMeshInterface interface implementation - - virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0); - - virtual void 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=0) const; - - /// 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) {(void) subpart;} - - 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; + void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2); + int getNumTriangles() const; + 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 ed81897b515..0a2c77096d1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -20,12 +20,18 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "stdio.h" btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) : m_meshInterface(meshInterface) { - recalcLocalAabb(); + if(meshInterface->hasPremadeAabb()) + { + meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax); + } + else + { + recalcLocalAabb(); + } } @@ -41,6 +47,7 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt { btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); btMatrix3x3 abs_b = trans.getBasis().absolute(); @@ -50,12 +57,10 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), abs_b[1].dot(localHalfExtents), abs_b[2].dot(localHalfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); - aabbMin = center - extent; aabbMax = center + extent; - + } void btTriangleMeshShape::recalcLocalAabb() @@ -138,6 +143,7 @@ const btVector3& btTriangleMeshShape::getLocalScaling() const //#define DEBUG_TRIANGLE_MESH + void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { struct FilteredCallback : public btInternalTriangleIndexCallback @@ -174,8 +180,7 @@ void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const - -void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { (void)mass; //moving concave objects not supported diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index e6173e47640..c9eabafe290 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -20,18 +20,20 @@ subject to the following restrictions: #include "btStridingMeshInterface.h" -///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. +///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead. class btTriangleMeshShape : public btConcaveShape { protected: btVector3 m_localAabbMin; btVector3 m_localAabbMax; btStridingMeshInterface* m_meshInterface; - -public: + ///btTriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class. + ///Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead! btTriangleMeshShape(btStridingMeshInterface* meshInterface); +public: + virtual ~btTriangleMeshShape(); virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; @@ -44,16 +46,11 @@ public: void recalcLocalAabb(); - virtual int getShapeType() const - { - return TRIANGLE_MESH_SHAPE_PROXYTYPE; - } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; @@ -68,9 +65,19 @@ public: return m_meshInterface; } + const btVector3& getLocalAabbMin() const + { + return m_localAabbMin; + } + const btVector3& getLocalAabbMax() const + { + return m_localAabbMax; + } + + //debugging - virtual char* getName()const {return "TRIANGLEMESH";} + virtual const char* getName()const {return "TRIANGLEMESH";} }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index c2e240c051c..ba7aa0c32a0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -27,7 +27,6 @@ public: btVector3 m_vertices1[3]; - virtual int getNumVertices() const { return 3; @@ -84,14 +83,13 @@ public: - btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) - { - m_vertices1[0] = p0; - m_vertices1[1] = p1; - m_vertices1[2] = p2; - } + btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) + { + m_vertices1[0] = p0; + m_vertices1[1] = p1; + m_vertices1[2] = p2; + } - virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const { @@ -116,7 +114,7 @@ public: planeSupport = m_vertices1[0]; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const { (void)mass; btAssert(0); @@ -155,7 +153,7 @@ public: return false; } //debugging - virtual char* getName()const + virtual const char* getName()const { return "Triangle"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp new file mode 100644 index 00000000000..ef340286cb0 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -0,0 +1,114 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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 "btUniformScalingShape.h" + +btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor): +m_childConvexShape(convexChildShape), +m_uniformScalingFactor(uniformScalingFactor) +{ +} + +btUniformScalingShape::~btUniformScalingShape() +{ +} + + +btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + btVector3 tmpVertex; + tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); + return tmpVertex*m_uniformScalingFactor; +} + +void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + int i; + for (i=0;i<numVectors;i++) + { + supportVerticesOut[i] = supportVerticesOut[i] * m_uniformScalingFactor; + } +} + + +btVector3 btUniformScalingShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 tmpVertex; + tmpVertex = m_childConvexShape->localGetSupportingVertex(vec); + return tmpVertex*m_uniformScalingFactor; +} + + +void btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + + ///this linear upscaling is not realistic, but we don't deal with large mass ratios... + btVector3 tmpInertia; + m_childConvexShape->calculateLocalInertia(mass,tmpInertia); + inertia = tmpInertia * m_uniformScalingFactor; +} + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btUniformScalingShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabb(t,aabbMin,aabbMax); + btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5); + btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor; + + aabbMin = aabbCenter - scaledAabbHalfExtends; + aabbMax = aabbCenter + scaledAabbHalfExtends; + +} + +void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); + btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5); + btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor; + + aabbMin = aabbCenter - scaledAabbHalfExtends; + aabbMax = aabbCenter + scaledAabbHalfExtends; +} + +void btUniformScalingShape::setLocalScaling(const btVector3& scaling) +{ + m_childConvexShape->setLocalScaling(scaling); +} + +const btVector3& btUniformScalingShape::getLocalScaling() const +{ + return m_childConvexShape->getLocalScaling(); +} + +void btUniformScalingShape::setMargin(btScalar margin) +{ + m_childConvexShape->setMargin(margin); +} +btScalar btUniformScalingShape::getMargin() const +{ + return m_childConvexShape->getMargin() * m_uniformScalingFactor; +} + +int btUniformScalingShape::getNumPreferredPenetrationDirections() const +{ + return m_childConvexShape->getNumPreferredPenetrationDirections(); +} + +void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const +{ + m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h new file mode 100644 index 00000000000..1e17fc8e198 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -0,0 +1,88 @@ +/* +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_UNIFORM_SCALING_SHAPE_H +#define BT_UNIFORM_SCALING_SHAPE_H + +#include "btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +///The btUniformScalingShape allows to re-use uniform scaled instances of btConvexShape in a memory efficient way. +///Istead of using btUniformScalingShape, it is better to use the non-uniform setLocalScaling method on convex shapes that implement it. +class btUniformScalingShape : public btConvexShape +{ + btConvexShape* m_childConvexShape; + + btScalar m_uniformScalingFactor; + + public: + + btUniformScalingShape( btConvexShape* convexChildShape, btScalar uniformScalingFactor); + + virtual ~btUniformScalingShape(); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + btScalar getUniformScalingFactor() const + { + return m_uniformScalingFactor; + } + + btConvexShape* getChildShape() + { + return m_childConvexShape; + } + + const btConvexShape* getChildShape() const + { + return m_childConvexShape; + } + + virtual const char* getName()const + { + return "UniformScalingShape"; + } + + virtual int getShapeType() const { return UNIFORM_SCALING_SHAPE_PROXYTYPE; } + + + /////////////////////////// + + + ///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; + + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void setLocalScaling(const btVector3& scaling) ; + virtual const btVector3& getLocalScaling() const ; + + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual int getNumPreferredPenetrationDirections() const; + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + + +}; + +#endif //BT_UNIFORM_SCALING_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h new file mode 100644 index 00000000000..cf43f487e24 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h @@ -0,0 +1,651 @@ +#ifndef BT_BOX_COLLISION_H_INCLUDED +#define BT_BOX_COLLISION_H_INCLUDED + +/*! \file gim_box_collision.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "LinearMath/btTransform.h" + +/*! \defgroup BOUND_AABB_OPERATIONS +*/ +//! @{ + +///Swap numbers +#define BT_SWAP_NUMBERS(a,b){ \ + a = a+b; \ + b = a-b; \ + a = a-b; \ +}\ + + +#define BT_MAX(a,b) (a<b?b:a) +#define BT_MIN(a,b) (a>b?b:a) + +#define BT_GREATER(x, y) fabsf(x) > (y) + +#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c)) +#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c)) + + + + + + +enum eBT_PLANE_INTERSECTION_TYPE +{ + BT_CONST_BACK_PLANE = 0, + BT_CONST_COLLIDE_PLANE, + BT_CONST_FRONT_PLANE +}; + +//SIMD_FORCE_INLINE bool test_cross_edge_box( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, const btVector3 & extend, +// int dir_index0, +// int dir_index1 +// int component_index0, +// int component_index1) +//{ +// // dir coords are -z and y +// +// const btScalar dir0 = -edge[dir_index0]; +// const btScalar dir1 = edge[dir_index1]; +// btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1; +// btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1; +// //find minmax +// if(pmin>pmax) +// { +// BT_SWAP_NUMBERS(pmin,pmax); +// } +// //find extends +// const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] + +// extend[component_index1] * absolute_edge[dir_index1]; +// +// if(pmin>rad || -rad>pmax) return false; +// return true; +//} +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2); +//} +// +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0); +//} +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1); +//} + + +#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\ +{\ + const btScalar dir0 = -edge[i_dir_0];\ + const btScalar dir1 = edge[i_dir_1];\ + btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\ + btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\ + if(pmin>pmax)\ + {\ + BT_SWAP_NUMBERS(pmin,pmax); \ + }\ + const btScalar abs_dir0 = absolute_edge[i_dir_0];\ + const btScalar abs_dir1 = absolute_edge[i_dir_1];\ + const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\ + if(pmin>rad || -rad>pmax) return false;\ +}\ + + +#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ +{\ + TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\ +}\ + +#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ +{\ + TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\ +}\ + +#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ +{\ + TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\ +}\ + + +//! Returns the dot product between a vec3f and the col of a matrix +SIMD_FORCE_INLINE btScalar bt_mat3_dot_col( +const btMatrix3x3 & mat, const btVector3 & vec3, int colindex) +{ + return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex]; +} + + +//! Class for transforming a model1 to the space of model0 +ATTRIBUTE_ALIGNED16 (class) BT_BOX_BOX_TRANSFORM_CACHE +{ +public: + btVector3 m_T1to0;//!< Transforms translation of model1 to model 0 + btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1 + btMatrix3x3 m_AR;//!< Absolute value of m_R1to0 + + SIMD_FORCE_INLINE void calc_absolute_matrix() + { +// static const btVector3 vepsi(1e-6f,1e-6f,1e-6f); +// m_AR[0] = vepsi + m_R1to0[0].absolute(); +// m_AR[1] = vepsi + m_R1to0[1].absolute(); +// m_AR[2] = vepsi + m_R1to0[2].absolute(); + + int i,j; + + for(i=0;i<3;i++) + { + for(j=0;j<3;j++ ) + { + m_AR[i][j] = 1e-6f + fabsf(m_R1to0[i][j]); + } + } + + } + + BT_BOX_BOX_TRANSFORM_CACHE() + { + } + + + + //! Calc the transformation relative 1 to 0. Inverts matrics by transposing + SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1) + { + + btTransform temp_trans = trans0.inverse(); + temp_trans = temp_trans * trans1; + + m_T1to0 = temp_trans.getOrigin(); + m_R1to0 = temp_trans.getBasis(); + + + calc_absolute_matrix(); + } + + //! Calcs the full invertion of the matrices. Useful for scaling matrices + SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1) + { + m_R1to0 = trans0.getBasis().inverse(); + m_T1to0 = m_R1to0 * (-trans0.getOrigin()); + + m_T1to0 += m_R1to0*trans1.getOrigin(); + m_R1to0 *= trans1.getBasis(); + + calc_absolute_matrix(); + } + + SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const + { + return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(), + m_R1to0[1].dot(point) + m_T1to0.y(), + m_R1to0[2].dot(point) + m_T1to0.z()); + } +}; + + +#define BOX_PLANE_EPSILON 0.000001f + +//! Axis aligned box +ATTRIBUTE_ALIGNED16 (class) btAABB +{ +public: + btVector3 m_min; + btVector3 m_max; + + btAABB() + {} + + + btAABB(const btVector3 & V1, + const btVector3 & V2, + const btVector3 & V3) + { + m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + } + + btAABB(const btVector3 & V1, + const btVector3 & V2, + const btVector3 & V3, + btScalar margin) + { + m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + btAABB(const btAABB &other): + m_min(other.m_min),m_max(other.m_max) + { + } + + btAABB(const btAABB &other,btScalar margin ): + m_min(other.m_min),m_max(other.m_max) + { + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + SIMD_FORCE_INLINE void invalidate() + { + m_min[0] = SIMD_INFINITY; + m_min[1] = SIMD_INFINITY; + m_min[2] = SIMD_INFINITY; + m_max[0] = -SIMD_INFINITY; + m_max[1] = -SIMD_INFINITY; + m_max[2] = -SIMD_INFINITY; + } + + SIMD_FORCE_INLINE void increment_margin(btScalar margin) + { + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin) + { + m_min[0] = other.m_min[0] - margin; + m_min[1] = other.m_min[1] - margin; + m_min[2] = other.m_min[2] - margin; + + m_max[0] = other.m_max[0] + margin; + m_max[1] = other.m_max[1] + margin; + m_max[2] = other.m_max[2] + margin; + } + + template<typename CLASS_POINT> + SIMD_FORCE_INLINE void calc_from_triangle( + const CLASS_POINT & V1, + const CLASS_POINT & V2, + const CLASS_POINT & V3) + { + m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + } + + template<typename CLASS_POINT> + SIMD_FORCE_INLINE void calc_from_triangle_margin( + const CLASS_POINT & V1, + const CLASS_POINT & V2, + const CLASS_POINT & V3, btScalar margin) + { + m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + //! Apply a transform to an AABB + SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extends = m_max - center; + // Compute new center + center = trans(center); + + btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()), + extends.dot(trans.getBasis().getRow(1).absolute()), + extends.dot(trans.getBasis().getRow(2).absolute())); + + m_min = center - textends; + m_max = center + textends; + } + + + //! Apply a transform to an AABB + SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans) + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extends = m_max - center; + // Compute new center + center = trans.transform(center); + + btVector3 textends(extends.dot(trans.m_R1to0.getRow(0).absolute()), + extends.dot(trans.m_R1to0.getRow(1).absolute()), + extends.dot(trans.m_R1to0.getRow(2).absolute())); + + m_min = center - textends; + m_max = center + textends; + } + + //! Merges a Box + SIMD_FORCE_INLINE void merge(const btAABB & box) + { + m_min[0] = BT_MIN(m_min[0],box.m_min[0]); + m_min[1] = BT_MIN(m_min[1],box.m_min[1]); + m_min[2] = BT_MIN(m_min[2],box.m_min[2]); + + m_max[0] = BT_MAX(m_max[0],box.m_max[0]); + m_max[1] = BT_MAX(m_max[1],box.m_max[1]); + m_max[2] = BT_MAX(m_max[2],box.m_max[2]); + } + + //! Merges a point + template<typename CLASS_POINT> + SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point) + { + m_min[0] = BT_MIN(m_min[0],point[0]); + m_min[1] = BT_MIN(m_min[1],point[1]); + m_min[2] = BT_MIN(m_min[2],point[2]); + + m_max[0] = BT_MAX(m_max[0],point[0]); + m_max[1] = BT_MAX(m_max[1],point[1]); + m_max[2] = BT_MAX(m_max[2],point[2]); + } + + //! Gets the extend and center + SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const + { + center = (m_max+m_min)*0.5f; + extend = m_max - center; + } + + //! Finds the intersecting box between this box and the other. + SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection) const + { + intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]); + intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]); + intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]); + + intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]); + intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]); + intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]); + } + + + SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const + { + if(m_min[0] > other.m_max[0] || + m_max[0] < other.m_min[0] || + m_min[1] > other.m_max[1] || + m_max[1] < other.m_min[1] || + m_min[2] > other.m_max[2] || + m_max[2] < other.m_min[2]) + { + return false; + } + return true; + } + + /*! \brief Finds the Ray intersection parameter. + \param aabb Aligned box + \param vorigin A vec3f with the origin of the ray + \param vdir A vec3f with the direction of the ray + */ + SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir) const + { + btVector3 extents,center; + this->get_center_extend(center,extents);; + + btScalar Dx = vorigin[0] - center[0]; + if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false; + btScalar Dy = vorigin[1] - center[1]; + if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false; + btScalar Dz = vorigin[2] - center[2]; + if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false; + + + btScalar f = vdir[1] * Dz - vdir[2] * Dy; + if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false; + f = vdir[2] * Dx - vdir[0] * Dz; + if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false; + f = vdir[0] * Dy - vdir[1] * Dx; + if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false; + return true; + } + + + SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extend = m_max-center; + + btScalar _fOrigin = direction.dot(center); + btScalar _fMaximumExtent = extend.dot(direction.absolute()); + vmin = _fOrigin - _fMaximumExtent; + vmax = _fOrigin + _fMaximumExtent; + } + + SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const + { + btScalar _fmin,_fmax; + this->projection_interval(plane,_fmin,_fmax); + + if(plane[3] > _fmax + BOX_PLANE_EPSILON) + { + return BT_CONST_BACK_PLANE; // 0 + } + + if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + { + return BT_CONST_COLLIDE_PLANE; //1 + } + return BT_CONST_FRONT_PLANE;//2 + } + + SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const + { + btAABB tbox = box; + tbox.appy_transform(trans1_to_0); + return has_collision(tbox); + } + + SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box, + const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const + { + btAABB tbox = box; + tbox.appy_transform_trans_cache(trans1_to_0); + return has_collision(tbox); + } + + //! transcache is the transformation cache from box to this AABB + SIMD_FORCE_INLINE bool overlapping_trans_cache( + const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const + { + + //Taken from OPCODE + btVector3 ea,eb;//extends + btVector3 ca,cb;//extends + get_center_extend(ca,ea); + box.get_center_extend(cb,eb); + + + btVector3 T; + btScalar t,t2; + int i; + + // Class I : A's basis vectors + for(i=0;i<3;i++) + { + T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; + t = transcache.m_AR[i].dot(eb) + ea[i]; + if(BT_GREATER(T[i], t)) return false; + } + // Class II : B's basis vectors + for(i=0;i<3;i++) + { + t = bt_mat3_dot_col(transcache.m_R1to0,T,i); + t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i]; + if(BT_GREATER(t,t2)) return false; + } + // Class III : 9 cross products + if(fulltest) + { + int j,m,n,o,p,q,r; + for(i=0;i<3;i++) + { + m = (i+1)%3; + n = (i+2)%3; + o = i==0?1:0; + p = i==2?1:2; + for(j=0;j<3;j++) + { + q = j==2?1:2; + r = j==0?1:0; + t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j]; + t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] + + eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r]; + if(BT_GREATER(t,t2)) return false; + } + } + } + return true; + } + + //! Simple test for planes. + SIMD_FORCE_INLINE bool collide_plane( + const btVector4 & plane) const + { + eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane); + return (classify == BT_CONST_COLLIDE_PLANE); + } + + //! test for a triangle, with edges + SIMD_FORCE_INLINE bool collide_triangle_exact( + const btVector3 & p1, + const btVector3 & p2, + const btVector3 & p3, + const btVector4 & triangle_plane) const + { + if(!collide_plane(triangle_plane)) return false; + + btVector3 center,extends; + this->get_center_extend(center,extends); + + const btVector3 v1(p1 - center); + const btVector3 v2(p2 - center); + const btVector3 v3(p3 - center); + + //First axis + btVector3 diff(v2 - v1); + btVector3 abs_diff = diff.absolute(); + //Test With X axis + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends); + //Test With Y axis + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends); + //Test With Z axis + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends); + + + diff = v3 - v2; + abs_diff = diff.absolute(); + //Test With X axis + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends); + //Test With Y axis + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends); + //Test With Z axis + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends); + + diff = v1 - v3; + abs_diff = diff.absolute(); + //Test With X axis + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends); + //Test With Y axis + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends); + //Test With Z axis + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends); + + return true; + } +}; + + +//! Compairison of transformation objects +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) +{ + if(!(t1.getOrigin() == t2.getOrigin()) ) return false; + + if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false; + if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false; + if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false; + return true; +} + + +//! @} + +#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h new file mode 100644 index 00000000000..28c03b7e039 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h @@ -0,0 +1,186 @@ +#ifndef BT_CLIP_POLYGON_H_INCLUDED +#define BT_CLIP_POLYGON_H_INCLUDED + +/*! \file btClipPolygon.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "LinearMath/btTransform.h" +#include "LinearMath/btGeometryUtil.h" + +/*! \addtogroup GEOMETRIC_OPERATIONS +*/ +//! @{ + +SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point) +{ + return point.dot(plane) - plane[3]; +} + +/*! Vector blending +Takes two vectors a, b, blends them together*/ +SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor) +{ + vr = (1-blend_factor)*va + blend_factor*vb; +} + +//! This function calcs the distance from a 3D plane +SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect( + const btVector3 & point0, + const btVector3 & point1, + btScalar dist0, + btScalar dist1, + btVector3 * clipped, + int & clipped_count) +{ + bool _prevclassif = (dist0>SIMD_EPSILON); + bool _classif = (dist1>SIMD_EPSILON); + if(_classif!=_prevclassif) + { + btScalar blendfactor = -dist0/(dist1-dist0); + bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor); + clipped_count++; + } + if(!_classif) + { + clipped[clipped_count] = point1; + clipped_count++; + } +} + + +//! Clips a polygon by a plane +/*! +*\return The count of the clipped counts +*/ +SIMD_FORCE_INLINE int bt_plane_clip_polygon( + const btVector4 & plane, + const btVector3 * polygon_points, + int polygon_point_count, + btVector3 * clipped) +{ + int clipped_count = 0; + + + //clip first point + btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);; + if(!(firstdist>SIMD_EPSILON)) + { + clipped[clipped_count] = polygon_points[0]; + clipped_count++; + } + + btScalar olddist = firstdist; + for(int i=1;i<polygon_point_count;i++) + { + btScalar dist = bt_distance_point_plane(plane,polygon_points[i]); + + bt_plane_clip_polygon_collect( + polygon_points[i-1],polygon_points[i], + olddist, + dist, + clipped, + clipped_count); + + + olddist = dist; + } + + //RETURN TO FIRST point + + bt_plane_clip_polygon_collect( + polygon_points[polygon_point_count-1],polygon_points[0], + olddist, + firstdist, + clipped, + clipped_count); + + return clipped_count; +} + +//! Clips a polygon by a plane +/*! +*\param clipped must be an array of 16 points. +*\return The count of the clipped counts +*/ +SIMD_FORCE_INLINE int bt_plane_clip_triangle( + const btVector4 & plane, + const btVector3 & point0, + const btVector3 & point1, + const btVector3& point2, + btVector3 * clipped // an allocated array of 16 points at least + ) +{ + int clipped_count = 0; + + //clip first point0 + btScalar firstdist = bt_distance_point_plane(plane,point0);; + if(!(firstdist>SIMD_EPSILON)) + { + clipped[clipped_count] = point0; + clipped_count++; + } + + // point 1 + btScalar olddist = firstdist; + btScalar dist = bt_distance_point_plane(plane,point1); + + bt_plane_clip_polygon_collect( + point0,point1, + olddist, + dist, + clipped, + clipped_count); + + olddist = dist; + + + // point 2 + dist = bt_distance_point_plane(plane,point2); + + bt_plane_clip_polygon_collect( + point1,point2, + olddist, + dist, + clipped, + clipped_count); + olddist = dist; + + + + //RETURN TO FIRST point0 + bt_plane_clip_polygon_collect( + point2,point0, + olddist, + firstdist, + clipped, + clipped_count); + + return clipped_count; +} + + + + +//! @} + +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp new file mode 100644 index 00000000000..faba2d8a542 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp @@ -0,0 +1,181 @@ + +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btContactProcessing.h" + +#define MAX_COINCIDENT 8 + +struct CONTACT_KEY_TOKEN +{ + unsigned int m_key; + int m_value; + CONTACT_KEY_TOKEN() + { + } + + CONTACT_KEY_TOKEN(unsigned int key,int token) + { + m_key = key; + m_value = token; + } + + CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken) + { + m_key = rtoken.m_key; + m_value = rtoken.m_value; + } + + inline bool operator <(const CONTACT_KEY_TOKEN& other) const + { + return (m_key < other.m_key); + } + + inline bool operator >(const CONTACT_KEY_TOKEN& other) const + { + return (m_key > other.m_key); + } + +}; + +class CONTACT_KEY_TOKEN_COMP +{ + public: + + bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) + { + return ( a < b ); + } +}; + + +void btContactArray::merge_contacts( + const btContactArray & contacts, bool normal_contact_average) +{ + clear(); + + int i; + if(contacts.size()==0) return; + + + if(contacts.size()==1) + { + push_back(contacts[0]); + return; + } + + btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts; + + keycontacts.reserve(contacts.size()); + + //fill key contacts + + for ( i = 0;i<contacts.size() ;i++ ) + { + keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i)); + } + + //sort keys + keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP()); + + // Merge contacts + int coincident_count=0; + btVector3 coincident_normals[MAX_COINCIDENT]; + + unsigned int last_key = keycontacts[0].m_key; + unsigned int key = 0; + + push_back(contacts[keycontacts[0].m_value]); + + BT_CONTACT * pcontact = &(*this)[0]; + + for( i=1;i<keycontacts.size();i++) + { + key = keycontacts[i].m_key; + const BT_CONTACT * scontact = &contacts[keycontacts[i].m_value]; + + if(last_key == key)//same points + { + //merge contact + if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + { + *pcontact = *scontact; + coincident_count = 0; + } + else if(normal_contact_average) + { + if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON) + { + if(coincident_count<MAX_COINCIDENT) + { + coincident_normals[coincident_count] = scontact->m_normal; + coincident_count++; + } + } + } + } + else + {//add new contact + + if(normal_contact_average && coincident_count>0) + { + pcontact->interpolate_normals(coincident_normals,coincident_count); + coincident_count = 0; + } + + push_back(*scontact); + pcontact = &(*this)[this->size()-1]; + } + last_key = key; + } +} + +void btContactArray::merge_contacts_unique(const btContactArray & contacts) +{ + clear(); + + if(contacts.size()==0) return; + + if(contacts.size()==1) + { + push_back(contacts[0]); + return; + } + + BT_CONTACT average_contact = contacts[0]; + + for (int i=1;i<contacts.size() ;i++ ) + { + average_contact.m_point += contacts[i].m_point; + average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth; + } + + //divide + btScalar divide_average = 1.0f/((btScalar)contacts.size()); + + average_contact.m_point *= divide_average; + + average_contact.m_normal *= divide_average; + + average_contact.m_depth = average_contact.m_normal.length(); + + average_contact.m_normal /= average_contact.m_depth; + +} + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h new file mode 100644 index 00000000000..0cfd013b962 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h @@ -0,0 +1,149 @@ +#ifndef BT_CONTACT_H_INCLUDED +#define BT_CONTACT_H_INCLUDED + +/*! \file gim_contact.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btTriangleShapeEx.h" + + +/*! \defgroup CONTACTS +\brief +Functions for managing and sorting contacts resulting from a collision query. +*/ +//! @{ + +/** +Configuration var for applying interpolation of contact normals +*/ +#define NORMAL_CONTACT_AVERAGE 1 + +#define CONTACT_DIFF_EPSILON 0.00001f + +/// Structure for collision results +class BT_CONTACT +{ +public: + btVector3 m_point; + btVector3 m_normal; + btScalar m_depth;//Positive value indicates interpenetration + btScalar m_distance;//Padding not for use + int m_feature1;//Face number + int m_feature2;//Face number +public: + BT_CONTACT() + { + } + + BT_CONTACT(const BT_CONTACT & contact): + m_point(contact.m_point), + m_normal(contact.m_normal), + m_depth(contact.m_depth), + m_feature1(contact.m_feature1), + m_feature2(contact.m_feature2) + { + } + + BT_CONTACT(const btVector3 &point,const btVector3 & normal, + btScalar depth, int feature1, int feature2): + m_point(point), + m_normal(normal), + m_depth(depth), + m_feature1(feature1), + m_feature2(feature2) + { + } + + //! Calcs key for coord classification + SIMD_FORCE_INLINE unsigned int calc_key_contact() const + { + int _coords[] = { + (int)(m_point[0]*1000.0f+1.0f), + (int)(m_point[1]*1333.0f), + (int)(m_point[2]*2133.0f+3.0f)}; + unsigned int _hash=0; + unsigned int *_uitmp = (unsigned int *)(&_coords[0]); + _hash = *_uitmp; + _uitmp++; + _hash += (*_uitmp)<<4; + _uitmp++; + _hash += (*_uitmp)<<8; + return _hash; + } + + SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count) + { + btVector3 vec_sum(m_normal); + for(int i=0;i<normal_count;i++) + { + vec_sum += normals[i]; + } + + btScalar vec_sum_len = vec_sum.length2(); + if(vec_sum_len <CONTACT_DIFF_EPSILON) return; + + //GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len) + + m_normal = vec_sum/btSqrt(vec_sum_len); + } + +}; + + +class btContactArray:public btAlignedObjectArray<BT_CONTACT> +{ +public: + btContactArray() + { + reserve(64); + } + + SIMD_FORCE_INLINE void push_contact( + const btVector3 &point,const btVector3 & normal, + btScalar depth, int feature1, int feature2) + { + push_back( BT_CONTACT(point,normal,depth,feature1,feature2) ); + } + + SIMD_FORCE_INLINE void push_triangle_contacts( + const BT_TRIANGLE_CONTACT & tricontact, + int feature1,int feature2) + { + for(int i = 0;i<tricontact.m_point_count ;i++ ) + { + push_contact( + tricontact.m_points[i], + tricontact.m_separating_normal, + tricontact.m_penetration_depth,feature1,feature2); + } + } + + void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true); + + void merge_contacts_unique(const btContactArray & contacts); +}; + +//! @} +#endif // GIM_CONTACT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp new file mode 100644 index 00000000000..b07926c30dc --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp @@ -0,0 +1,499 @@ +/*! \file gim_box_set.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btGImpactBvh.h" +#include "LinearMath/btQuickprof.h" + +#ifdef TRI_COLLISION_PROFILING + +btClock g_tree_clock; + +float g_accum_tree_collision_time = 0; +int g_count_traversing = 0; + + +void bt_begin_gim02_tree_time() +{ + g_tree_clock.reset(); +} + +void bt_end_gim02_tree_time() +{ + g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds(); + g_count_traversing++; +} + +//! Gets the average time in miliseconds of tree collisions +float btGImpactBvh::getAverageTreeCollisionTime() +{ + if(g_count_traversing == 0) return 0; + + float avgtime = g_accum_tree_collision_time; + avgtime /= (float)g_count_traversing; + + g_accum_tree_collision_time = 0; + g_count_traversing = 0; + return avgtime; + +// float avgtime = g_count_traversing; +// g_count_traversing = 0; +// return avgtime; + +} + +#endif //TRI_COLLISION_PROFILING + +/////////////////////// btBvhTree ///////////////////////////////// + +int btBvhTree::_calc_splitting_axis( + BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +{ + + int i; + + 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 = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means+=center; + } + means *= (btScalar(1.)/(btScalar)numIndices); + + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + btVector3 diff2 = center-means; + diff2 = diff2 * diff2; + variance += diff2; + } + variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + + return variance.maxAxis(); +} + + +int btBvhTree::_sort_and_calc_splitting_index( + BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + int endIndex, int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + + // average of centers + btScalar splitValue = 0.0f; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means+=center; + } + 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 = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + if (center[splitAxis] > splitValue) + { + //swap + primitive_boxes.swap(i,splitIndex); + //swapLeafNodes(i,splitIndex); + splitIndex++; + } + } + + //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; + +} + + +void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +{ + int curIndex = m_num_nodes; + m_num_nodes++; + + btAssert((endIndex-startIndex)>0); + + if ((endIndex-startIndex)==1) + { + //We have a leaf node + setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); + + return; + } + //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + + //split axis + int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + + splitIndex = _sort_and_calc_splitting_index( + primitive_boxes,startIndex,endIndex, + splitIndex//split axis + ); + + + //calc this node bounding box + + btAABB node_bound; + node_bound.invalidate(); + + for (int i=startIndex;i<endIndex;i++) + { + node_bound.merge(primitive_boxes[i].m_bound); + } + + setNodeBound(curIndex,node_bound); + + + //build left branch + _build_sub_tree(primitive_boxes, startIndex, splitIndex ); + + + //build right branch + _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + + m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); + + +} + +//! stackless build tree +void btBvhTree::build_tree( + BT_BVH_DATA_ARRAY & primitive_boxes) +{ + // initialize node count to 0 + m_num_nodes = 0; + // allocate nodes + m_node_array.resize(primitive_boxes.size()*2); + + _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); +} + +////////////////////////////////////class btGImpactBvh + +void btGImpactBvh::refit() +{ + int nodecount = getNodeCount(); + while(nodecount--) + { + if(isLeafNode(nodecount)) + { + btAABB leafbox; + m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); + setNodeBound(nodecount,leafbox); + } + else + { + //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //get left bound + btAABB bound; + bound.invalidate(); + + btAABB temp_box; + + int child_node = getLeftNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + child_node = getRightNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + setNodeBound(nodecount,bound); + } + } +} + +//! this rebuild the entire set +void btGImpactBvh::buildSet() +{ + //obtain primitive boxes + BT_BVH_DATA_ARRAY primitive_boxes; + primitive_boxes.resize(m_primitive_manager->get_primitive_count()); + + for (int i = 0;i<primitive_boxes.size() ;i++ ) + { + m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; + } + + m_box_tree.build_tree(primitive_boxes); +} + +//! returns the indices of the primitives in the m_primitive_manager +bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + btAABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.has_collision(box); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData(curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + if(collided_results.size()>0) return true; + return false; +} + + + +//! returns the indices of the primitives in the m_primitive_manager +bool btGImpactBvh::rayQuery( + const btVector3 & ray_dir,const btVector3 & ray_origin , + btAlignedObjectArray<int> & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + btAABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData( curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + if(collided_results.size()>0) return true; + return false; +} + + +SIMD_FORCE_INLINE bool _node_collision( + btGImpactBvh * boxset0, btGImpactBvh * boxset1, + const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + int node0 ,int node1, bool complete_primitive_tests) +{ + btAABB box0; + boxset0->getNodeBound(node0,box0); + btAABB box1; + boxset1->getNodeBound(node1,box1); + + return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); +// box1.appy_transform_trans_cache(trans_cache_1to0); +// return box0.has_collision(box1); + +} + + +//stackless recursive collision routine +static void _find_collision_pairs_recursive( + btGImpactBvh * boxset0, btGImpactBvh * boxset1, + btPairSet * collision_pairs, + const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + int node0, int node1, bool complete_primitive_tests) +{ + + + + if( _node_collision( + boxset0,boxset1,trans_cache_1to0, + node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes + + if(boxset0->isLeafNode(node0)) + { + if(boxset1->isLeafNode(node1)) + { + // collision result + collision_pairs->push_pair( + boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + return; + } + else + { + + //collide left recursive + + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + node0,boxset1->getLeftNode(node1),false); + + //collide right recursive + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + node0,boxset1->getRightNode(node1),false); + + + } + } + else + { + if(boxset1->isLeafNode(node1)) + { + + //collide left recursive + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getLeftNode(node0),node1,false); + + + //collide right recursive + + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getRightNode(node0),node1,false); + + + } + else + { + //collide left0 left1 + + + + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); + + //collide left0 right1 + + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); + + + //collide right0 left1 + + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); + + //collide right0 right1 + + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); + + }// else if node1 is not a leaf + }// else if node0 is not a leaf +} + + +void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0, + btGImpactBvh * boxset1, const btTransform & trans1, + btPairSet & collision_pairs) +{ + + if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + + BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; + + trans_cache_1to0.calc_from_homogenic(trans0,trans1); + +#ifdef TRI_COLLISION_PROFILING + bt_begin_gim02_tree_time(); +#endif //TRI_COLLISION_PROFILING + + _find_collision_pairs_recursive( + boxset0,boxset1, + &collision_pairs,trans_cache_1to0,0,0,true); +#ifdef TRI_COLLISION_PROFILING + bt_end_gim02_tree_time(); +#endif //TRI_COLLISION_PROFILING + +} + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h new file mode 100644 index 00000000000..1907efc2463 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h @@ -0,0 +1,401 @@ +#ifndef GIM_BOX_SET_H_INCLUDED +#define GIM_BOX_SET_H_INCLUDED + +/*! \file gim_box_set.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "LinearMath/btAlignedObjectArray.h" + +#include "btBoxCollision.h" +#include "btTriangleShapeEx.h" + + + +/*! \defgroup BOX_TREES + + + +*/ +//! @{ + + +//! Overlapping pair +struct BT_PAIR +{ + int m_index1; + int m_index2; + BT_PAIR() + {} + + BT_PAIR(const BT_PAIR & p) + { + m_index1 = p.m_index1; + m_index2 = p.m_index2; + } + + BT_PAIR(int index1, int index2) + { + m_index1 = index1; + m_index2 = index2; + } +}; + +//! A pairset array +class btPairSet: public btAlignedObjectArray<BT_PAIR> +{ +public: + btPairSet() + { + reserve(32); + } + inline void push_pair(int index1,int index2) + { + push_back(BT_PAIR(index1,index2)); + } + + inline void push_pair_inv(int index1,int index2) + { + push_back(BT_PAIR(index2,index1)); + } +}; + + + +struct BT_BVH_DATA +{ + btAABB m_bound; + int m_data; +}; + +//! Node Structure for trees +class BT_BVH_TREE_NODE +{ +public: + btAABB m_bound; +protected: + int m_escapeIndexOrDataIndex; +public: + BT_BVH_TREE_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + +}; + + +class BT_BVH_DATA_ARRAY:public btAlignedObjectArray<BT_BVH_DATA> +{ +}; + + +class BT_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<BT_BVH_TREE_NODE> +{ +}; + + + + +//! Basic Box tree structure +class btBvhTree +{ +protected: + int m_num_nodes; + BT_BVH_TREE_NODE_ARRAY m_node_array; +protected: + int _sort_and_calc_splitting_index( + BT_BVH_DATA_ARRAY & primitive_boxes, + int startIndex, int endIndex, int splitAxis); + + int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); +public: + btBvhTree() + { + m_num_nodes = 0; + } + + //! prototype functions for box tree management + //!@{ + void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes); + + SIMD_FORCE_INLINE void clearNodes() + { + m_node_array.clear(); + m_num_nodes = 0; + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_num_nodes; + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_node_array[nodeindex].isLeafNode(); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_node_array[nodeindex].getDataIndex(); + } + + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + { + bound = m_node_array[nodeindex].m_bound; + } + + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + { + m_node_array[nodeindex].m_bound = bound; + } + + SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const + { + return nodeindex+1; + } + + SIMD_FORCE_INLINE int getRightNode(int nodeindex) const + { + if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2; + return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex(); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_node_array[nodeindex].getEscapeIndex(); + } + + SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const + { + return &m_node_array[index]; + } + + //!@} +}; + + +//! Prototype Base class for primitive classification +/*! +This class is a wrapper for primitive collections. +This tells relevant info for the Bounding Box set classes, which take care of space classification. +This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons. +*/ +class btPrimitiveManagerBase +{ +public: + + //! determines if this manager consist on only triangles, which special case will be optimized + virtual bool is_trimesh() const = 0; + virtual int get_primitive_count() const = 0; + virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0; + //! retrieves only the points of the triangle, and the collision margin + virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0; +}; + + +//! Structure for containing Boxes +/*! +This class offers an structure for managing a box tree of primitives. +Requires a Primitive prototype (like btPrimitiveManagerBase ) +*/ +class btGImpactBvh +{ +protected: + btBvhTree m_box_tree; + btPrimitiveManagerBase * m_primitive_manager; + +protected: + //stackless refit + void refit(); +public: + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactBvh() + { + m_primitive_manager = NULL; + } + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactBvh(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btAABB getGlobalBox() const + { + btAABB totalbox; + getNodeBound(0, totalbox); + return totalbox; + } + + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + { + return m_primitive_manager; + } + + +//! node manager prototype functions +///@{ + + //! this attemps to refit the box set. + SIMD_FORCE_INLINE void update() + { + refit(); + } + + //! this rebuild the entire set + void buildSet(); + + //! returns the indices of the primitives in the m_primitive_manager + bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const; + + //! returns the indices of the primitives in the m_primitive_manager + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box, + const btTransform & transform, btAlignedObjectArray<int> & collided_results) const + { + btAABB transbox=box; + transbox.appy_transform(transform); + return boxQuery(transbox,collided_results); + } + + //! returns the indices of the primitives in the m_primitive_manager + bool rayQuery( + const btVector3 & ray_dir,const btVector3 & ray_origin , + btAlignedObjectArray<int> & collided_results) const; + + //! tells if this set has hierarcht + SIMD_FORCE_INLINE bool hasHierarchy() const + { + return true; + } + + //! tells if this set is a trimesh + SIMD_FORCE_INLINE bool isTrimesh() const + { + return m_primitive_manager->is_trimesh(); + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_box_tree.getNodeCount(); + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_box_tree.isLeafNode(nodeindex); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_box_tree.getNodeData(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + { + m_box_tree.getNodeBound(nodeindex, bound); + } + + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + { + m_box_tree.setNodeBound(nodeindex, bound); + } + + + SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const + { + return m_box_tree.getLeftNode(nodeindex); + } + + SIMD_FORCE_INLINE int getRightNode(int nodeindex) const + { + return m_box_tree.getRightNode(nodeindex); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_box_tree.getEscapeNodeIndex(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + { + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + } + + + SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const + { + return m_box_tree.get_node_pointer(index); + } + +//! @} + + static float getAverageTreeCollisionTime(); + + + static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1, + btGImpactBvh * boxset2, const btTransform & trans2, + btPairSet & collision_pairs); +}; + + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp new file mode 100644 index 00000000000..28ad18fbde2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -0,0 +1,896 @@ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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. +*/ +/* +Author: Francisco Len Nßjera +Concave-Concave Collision + +*/ + +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "btGImpactCollisionAlgorithm.h" +#include "btContactProcessing.h" +#include "LinearMath/btQuickprof.h" + + +//! Class for accessing the plane equation +class btPlaneShape : public btStaticPlaneShape +{ +public: + + btPlaneShape(const btVector3& v, float f) + :btStaticPlaneShape(v,f) + { + } + + void get_plane_equation(btVector4 &equation) + { + equation[0] = m_planeNormal[0]; + equation[1] = m_planeNormal[1]; + equation[2] = m_planeNormal[2]; + equation[3] = m_planeConstant; + } + + + void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) + { + equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal); + equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal); + equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal); + equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant; + } +}; + + + +////////////////////////////////////////////////////////////////////////////////////////////// +#ifdef TRI_COLLISION_PROFILING + +btClock g_triangle_clock; + +float g_accum_triangle_collision_time = 0; +int g_count_triangle_collision = 0; + +void bt_begin_gim02_tri_time() +{ + g_triangle_clock.reset(); +} + +void bt_end_gim02_tri_time() +{ + g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); + g_count_triangle_collision++; +} +#endif //TRI_COLLISION_PROFILING +//! Retrieving shapes shapes +/*! +Declared here due of insuficent space on Pool allocators +*/ +//!@{ +class GIM_ShapeRetriever +{ +public: + btGImpactShapeInterface * m_gim_shape; + btTriangleShapeEx m_trishape; + btTetrahedronShapeEx m_tetrashape; + +public: + class ChildShapeRetriever + { + public: + GIM_ShapeRetriever * m_parent; + virtual btCollisionShape * getChildShape(int index) + { + return m_parent->m_gim_shape->getChildShape(index); + } + }; + + class TriangleShapeRetriever:public ChildShapeRetriever + { + public: + + virtual btCollisionShape * getChildShape(int index) + { + m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); + return &m_parent->m_trishape; + } + }; + + class TetraShapeRetriever:public ChildShapeRetriever + { + public: + + virtual btCollisionShape * getChildShape(int index) + { + m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape); + return &m_parent->m_tetrashape; + } + }; +public: + ChildShapeRetriever m_child_retriever; + TriangleShapeRetriever m_tri_retriever; + TetraShapeRetriever m_tetra_retriever; + ChildShapeRetriever * m_current_retriever; + + GIM_ShapeRetriever(btGImpactShapeInterface * gim_shape) + { + m_gim_shape = gim_shape; + //select retriever + if(m_gim_shape->needsRetrieveTriangles()) + { + m_current_retriever = &m_tri_retriever; + } + else if(m_gim_shape->needsRetrieveTetrahedrons()) + { + m_current_retriever = &m_tetra_retriever; + } + else + { + m_current_retriever = &m_child_retriever; + } + + m_current_retriever->m_parent = this; + } + + btCollisionShape * getChildShape(int index) + { + return m_current_retriever->getChildShape(index); + } + + +}; + + + +//!@} + + +#ifdef TRI_COLLISION_PROFILING + +//! Gets the average time in miliseconds of tree collisions +float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime() +{ + return btGImpactBoxSet::getAverageTreeCollisionTime(); + +} + +//! Gets the average time in miliseconds of triangle collisions +float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() +{ + if(g_count_triangle_collision == 0) return 0; + + float avgtime = g_accum_triangle_collision_time; + avgtime /= (float)g_count_triangle_collision; + + g_accum_triangle_collision_time = 0; + g_count_triangle_collision = 0; + + return avgtime; +} + +#endif //TRI_COLLISION_PROFILING + + + +btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) +: btCollisionAlgorithm(ci) +{ + m_manifoldPtr = NULL; + m_convex_algorithm = NULL; +} + +btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() +{ + clearCache(); +} + + + + + +void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0, + btCollisionObject * body1, + const btVector3 & point, + const btVector3 & normal, + btScalar distance) +{ + m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); + checkManifold(body0,body1); + m_resultOut->addContactPoint(normal,point,distance); +} + + +void btGImpactCollisionAlgorithm::shape_vs_shape_collision( + btCollisionObject * body0, + btCollisionObject * body1, + btCollisionShape * shape0, + btCollisionShape * shape1) +{ + + btCollisionShape* tmpShape0 = body0->getCollisionShape(); + btCollisionShape* tmpShape1 = body1->getCollisionShape(); + + body0->internalSetTemporaryCollisionShape(shape0); + body1->internalSetTemporaryCollisionShape(shape1); + + { + btCollisionAlgorithm* algor = newAlgorithm(body0,body1); + // post : checkManifold is called + + m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); + + algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut); + + algor->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algor); + } + + body0->internalSetTemporaryCollisionShape(tmpShape0); + body1->internalSetTemporaryCollisionShape(tmpShape1); +} + +void btGImpactCollisionAlgorithm::convex_vs_convex_collision( + btCollisionObject * body0, + btCollisionObject * body1, + btCollisionShape * shape0, + btCollisionShape * shape1) +{ + + btCollisionShape* tmpShape0 = body0->getCollisionShape(); + btCollisionShape* tmpShape1 = body1->getCollisionShape(); + + body0->internalSetTemporaryCollisionShape(shape0); + body1->internalSetTemporaryCollisionShape(shape1); + + + m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); + + checkConvexAlgorithm(body0,body1); + m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut); + + body0->internalSetTemporaryCollisionShape(tmpShape0); + body1->internalSetTemporaryCollisionShape(tmpShape1); + +} + + + + +void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( + const btTransform & trans0, + const btTransform & trans1, + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1,btPairSet & pairset) +{ + if(shape0->hasBoxSet() && shape1->hasBoxSet()) + { + btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); + } + else + { + btAABB boxshape0; + btAABB boxshape1; + int i = shape0->getNumChildShapes(); + + while(i--) + { + shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + + int j = shape1->getNumChildShapes(); + while(j--) + { + shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); + + if(boxshape1.has_collision(boxshape0)) + { + pairset.push_pair(i,j); + } + } + } + } + + +} + + +void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( + const btTransform & trans0, + const btTransform & trans1, + btGImpactShapeInterface * shape0, + btCollisionShape * shape1, + btAlignedObjectArray<int> & collided_primitives) +{ + + btAABB boxshape; + + + if(shape0->hasBoxSet()) + { + btTransform trans1to0 = trans0.inverse(); + trans1to0 *= trans1; + + shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); + + shape0->getBoxSet()->boxQuery(boxshape, collided_primitives); + } + else + { + shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max); + + btAABB boxshape0; + int i = shape0->getNumChildShapes(); + + while(i--) + { + shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + + if(boxshape.has_collision(boxshape0)) + { + collided_primitives.push_back(i); + } + } + + } + +} + + +void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count) +{ + btTriangleShapeEx tri0; + btTriangleShapeEx tri1; + + shape0->lockChildShapes(); + shape1->lockChildShapes(); + + const int * pair_pointer = pairs; + + while(pair_count--) + { + + m_triface0 = *(pair_pointer); + m_triface1 = *(pair_pointer+1); + pair_pointer+=2; + + + + shape0->getBulletTriangle(m_triface0,tri0); + shape1->getBulletTriangle(m_triface1,tri1); + + + //collide two convex shapes + if(tri0.overlap_test_conservative(tri1)) + { + convex_vs_convex_collision(body0,body1,&tri0,&tri1); + } + + } + + shape0->unlockChildShapes(); + shape1->unlockChildShapes(); +} + +void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count) +{ + btTransform orgtrans0 = body0->getWorldTransform(); + btTransform orgtrans1 = body1->getWorldTransform(); + + btPrimitiveTriangle ptri0; + btPrimitiveTriangle ptri1; + BT_TRIANGLE_CONTACT contact_data; + + shape0->lockChildShapes(); + shape1->lockChildShapes(); + + const int * pair_pointer = pairs; + + while(pair_count--) + { + + m_triface0 = *(pair_pointer); + m_triface1 = *(pair_pointer+1); + pair_pointer+=2; + + + shape0->getPrimitiveTriangle(m_triface0,ptri0); + shape1->getPrimitiveTriangle(m_triface1,ptri1); + + #ifdef TRI_COLLISION_PROFILING + bt_begin_gim02_tri_time(); + #endif + + ptri0.applyTransform(orgtrans0); + ptri1.applyTransform(orgtrans1); + + + //build planes + ptri0.buildTriPlane(); + ptri1.buildTriPlane(); + // test conservative + + + + if(ptri0.overlap_test_conservative(ptri1)) + { + if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) + { + + int j = contact_data.m_point_count; + while(j--) + { + + addContactPoint(body0, body1, + contact_data.m_points[j], + contact_data.m_separating_normal, + -contact_data.m_penetration_depth); + } + } + } + + #ifdef TRI_COLLISION_PROFILING + bt_end_gim02_tri_time(); + #endif + + } + + shape0->unlockChildShapes(); + shape1->unlockChildShapes(); + +} + + +void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1) +{ + + if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + { + btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0); + m_part0 = meshshape0->getMeshPartCount(); + + while(m_part0--) + { + gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1); + } + + return; + } + + if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + { + btGImpactMeshShape * meshshape1 = static_cast<btGImpactMeshShape *>(shape1); + m_part1 = meshshape1->getMeshPartCount(); + + while(m_part1--) + { + + gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1)); + + } + + return; + } + + + btTransform orgtrans0 = body0->getWorldTransform(); + btTransform orgtrans1 = body1->getWorldTransform(); + + btPairSet pairset; + + gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); + + if(pairset.size()== 0) return; + + if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) + { + btGImpactMeshShapePart * shapepart0 = static_cast<btGImpactMeshShapePart * >(shape0); + btGImpactMeshShapePart * shapepart1 = static_cast<btGImpactMeshShapePart * >(shape1); + //specialized function + #ifdef BULLET_TRIANGLE_COLLISION + collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); + #else + collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); + #endif + + return; + } + + //general function + + shape0->lockChildShapes(); + shape1->lockChildShapes(); + + GIM_ShapeRetriever retriever0(shape0); + GIM_ShapeRetriever retriever1(shape1); + + bool child_has_transform0 = shape0->childrenHasTransform(); + bool child_has_transform1 = shape1->childrenHasTransform(); + + int i = pairset.size(); + while(i--) + { + BT_PAIR * pair = &pairset[i]; + m_triface0 = pair->m_index1; + m_triface1 = pair->m_index2; + btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); + btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); + + if(child_has_transform0) + { + body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0)); + } + + if(child_has_transform1) + { + body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1)); + } + + //collide two convex shapes + convex_vs_convex_collision(body0,body1,colshape0,colshape1); + + + if(child_has_transform0) + { + body0->setWorldTransform(orgtrans0); + } + + if(child_has_transform1) + { + body1->setWorldTransform(orgtrans1); + } + + } + + shape0->unlockChildShapes(); + shape1->unlockChildShapes(); +} + +void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btCollisionShape * shape1,bool swapped) +{ + if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + { + btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0); + m_part0 = meshshape0->getMeshPartCount(); + + while(m_part0--) + { + + gimpact_vs_shape(body0, + body1, + meshshape0->getMeshPart(m_part0), + shape1,swapped); + + } + + return; + } + + #ifdef GIMPACT_VS_PLANE_COLLISION + if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) + { + btGImpactMeshShapePart * shapepart = static_cast<btGImpactMeshShapePart *>(shape0); + btStaticPlaneShape * planeshape = static_cast<btStaticPlaneShape * >(shape1); + gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped); + return; + } + + #endif + + + + if(shape1->isCompound()) + { + btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1); + gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped); + return; + } + else if(shape1->isConcave()) + { + btConcaveShape * concaveshape = static_cast<btConcaveShape *>(shape1); + gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped); + return; + } + + + btTransform orgtrans0 = body0->getWorldTransform(); + + btTransform orgtrans1 = body1->getWorldTransform(); + + btAlignedObjectArray<int> collided_results; + + gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results); + + if(collided_results.size() == 0) return; + + + shape0->lockChildShapes(); + + GIM_ShapeRetriever retriever0(shape0); + + + bool child_has_transform0 = shape0->childrenHasTransform(); + + + int i = collided_results.size(); + + while(i--) + { + int child_index = collided_results[i]; + if(swapped) + m_triface1 = child_index; + else + m_triface0 = child_index; + + btCollisionShape * colshape0 = retriever0.getChildShape(child_index); + + if(child_has_transform0) + { + body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index)); + } + + //collide two shapes + if(swapped) + { + shape_vs_shape_collision(body1,body0,shape1,colshape0); + } + else + { + shape_vs_shape_collision(body0,body1,colshape0,shape1); + } + + //restore transforms + if(child_has_transform0) + { + body0->setWorldTransform(orgtrans0); + } + + } + + shape0->unlockChildShapes(); + +} + +void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btCompoundShape * shape1,bool swapped) +{ + btTransform orgtrans1 = body1->getWorldTransform(); + + int i = shape1->getNumChildShapes(); + while(i--) + { + + btCollisionShape * colshape1 = shape1->getChildShape(i); + btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); + + body1->setWorldTransform(childtrans1); + + //collide child shape + gimpact_vs_shape(body0, body1, + shape0,colshape1,swapped); + + + //restore transforms + body1->setWorldTransform(orgtrans1); + } +} + +void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btStaticPlaneShape * shape1,bool swapped) +{ + + + btTransform orgtrans0 = body0->getWorldTransform(); + btTransform orgtrans1 = body1->getWorldTransform(); + + btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1); + btVector4 plane; + planeshape->get_plane_equation_transformed(orgtrans1,plane); + + //test box against plane + + btAABB tribox; + shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); + tribox.increment_margin(planeshape->getMargin()); + + if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; + + shape0->lockChildShapes(); + + btScalar margin = shape0->getMargin() + planeshape->getMargin(); + + btVector3 vertex; + int vi = shape0->getVertexCount(); + while(vi--) + { + shape0->getVertex(vi,vertex); + vertex = orgtrans0(vertex); + + btScalar distance = vertex.dot(plane) - plane[3] - margin; + + if(distance<0.0)//add contact + { + if(swapped) + { + addContactPoint(body1, body0, + vertex, + -plane, + distance); + } + else + { + addContactPoint(body0, body1, + vertex, + plane, + distance); + } + } + } + + shape0->unlockChildShapes(); +} + + + + +class btGImpactTriangleCallback: public btTriangleCallback +{ +public: + btGImpactCollisionAlgorithm * algorithm; + btCollisionObject * body0; + btCollisionObject * body1; + btGImpactShapeInterface * gimpactshape0; + bool swapped; + btScalar margin; + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); + tri1.setMargin(margin); + if(swapped) + { + algorithm->setPart0(partId); + algorithm->setFace0(triangleIndex); + } + else + { + algorithm->setPart1(partId); + algorithm->setFace1(triangleIndex); + } + algorithm->gimpact_vs_shape( + body0,body1,gimpactshape0,&tri1,swapped); + } +}; + + + + +void btGImpactCollisionAlgorithm::gimpact_vs_concave( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btConcaveShape * shape1,bool swapped) +{ + //create the callback + btGImpactTriangleCallback tricallback; + tricallback.algorithm = this; + tricallback.body0 = body0; + tricallback.body1 = body1; + tricallback.gimpactshape0 = shape0; + tricallback.swapped = swapped; + tricallback.margin = shape1->getMargin(); + + //getting the trimesh AABB + btTransform gimpactInConcaveSpace; + + gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform(); + + btVector3 minAABB,maxAABB; + shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); + + shape1->processAllTriangles(&tricallback,minAABB,maxAABB); + +} + + + +void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + clearCache(); + + m_resultOut = resultOut; + m_dispatchInfo = &dispatchInfo; + btGImpactShapeInterface * gimpactshape0; + btGImpactShapeInterface * gimpactshape1; + + if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) + { + gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape()); + + if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + { + gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape()); + + gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1); + } + else + { + gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false); + } + + } + else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + { + gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape()); + + gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true); + } +} + + +btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + return 1.f; + +} + +///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// + +btGImpactCollisionAlgorithm::CreateFunc g_gimpact_cf; + +//! Use this function for register the algorithm externally +void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) +{ + + int i; + + for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + { + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&g_gimpact_cf); + } + + for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + { + dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&g_gimpact_cf); + } + +} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h new file mode 100644 index 00000000000..c745b7ed5d8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -0,0 +1,306 @@ +/*! \file btGImpactShape.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 BVH_CONCAVE_COLLISION_ALGORITHM_H +#define BVH_CONCAVE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" + +#include "LinearMath/btAlignedObjectArray.h" + +#include "btGImpactShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" +#include "LinearMath/btIDebugDraw.h" + + + +//! Collision Algorithm for GImpact Shapes +/*! +For register this algorithm in Bullet, proceed as following: + \code +btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(m_dynamicsWorld ->getDispatcher()); +btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); + \endcode +*/ +class btGImpactCollisionAlgorithm : public btCollisionAlgorithm +{ +protected: + btCollisionAlgorithm * m_convex_algorithm; + btPersistentManifold * m_manifoldPtr; + btManifoldResult* m_resultOut; + const btDispatcherInfo * m_dispatchInfo; + int m_triface0; + int m_part0; + int m_triface1; + int m_part1; + + + //! Creates a new contact point + SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(btCollisionObject* body0,btCollisionObject* body1) + { + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + return m_manifoldPtr; + } + + SIMD_FORCE_INLINE void destroyConvexAlgorithm() + { + if(m_convex_algorithm) + { + m_convex_algorithm->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm); + m_convex_algorithm = NULL; + } + } + + SIMD_FORCE_INLINE void destroyContactManifolds() + { + if(m_manifoldPtr == NULL) return; + m_dispatcher->releaseManifold(m_manifoldPtr); + m_manifoldPtr = NULL; + } + + SIMD_FORCE_INLINE void clearCache() + { + destroyContactManifolds(); + destroyConvexAlgorithm(); + + m_triface0 = -1; + m_part0 = -1; + m_triface1 = -1; + m_part1 = -1; + } + + SIMD_FORCE_INLINE btPersistentManifold* getLastManifold() + { + return m_manifoldPtr; + } + + + // Call before process collision + SIMD_FORCE_INLINE void checkManifold(btCollisionObject* body0,btCollisionObject* body1) + { + if(getLastManifold() == 0) + { + newContactManifold(body0,body1); + } + + m_resultOut->setPersistentManifold(getLastManifold()); + } + + // Call before process collision + SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(btCollisionObject* body0,btCollisionObject* body1) + { + checkManifold(body0,body1); + + btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( + body0,body1,getLastManifold()); + return convex_algorithm ; + } + + // Call before process collision + SIMD_FORCE_INLINE void checkConvexAlgorithm(btCollisionObject* body0,btCollisionObject* body1) + { + if(m_convex_algorithm) return; + m_convex_algorithm = newAlgorithm(body0,body1); + } + + + + + void addContactPoint(btCollisionObject * body0, + btCollisionObject * body1, + const btVector3 & point, + const btVector3 & normal, + btScalar distance); + +//! Collision routines +//!@{ + + void collide_gjk_triangles(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count); + + void collide_sat_triangles(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count); + + + + + void shape_vs_shape_collision( + btCollisionObject * body0, + btCollisionObject * body1, + btCollisionShape * shape0, + btCollisionShape * shape1); + + void convex_vs_convex_collision(btCollisionObject * body0, + btCollisionObject * body1, + btCollisionShape * shape0, + btCollisionShape * shape1); + + + + void gimpact_vs_gimpact_find_pairs( + const btTransform & trans0, + const btTransform & trans1, + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1,btPairSet & pairset); + + void gimpact_vs_shape_find_pairs( + const btTransform & trans0, + const btTransform & trans1, + btGImpactShapeInterface * shape0, + btCollisionShape * shape1, + btAlignedObjectArray<int> & collided_primitives); + + + void gimpacttrimeshpart_vs_plane_collision( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btStaticPlaneShape * shape1,bool swapped); + + +public: + + btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btGImpactCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr) + manifoldArray.push_back(m_manifoldPtr); + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm)); + return new(mem) btGImpactCollisionAlgorithm(ci,body0,body1); + } + }; + + //! Use this function for register the algorithm externally + static void registerAlgorithm(btCollisionDispatcher * dispatcher); + + //! Gets the average time in miliseconds of tree collisions + static float getAverageTreeCollisionTime(); + + //! Gets the average time in miliseconds of triangle collisions + static float getAverageTriangleCollisionTime(); + + + //! Collides two gimpact shapes + /*! + \pre shape0 and shape1 couldn't be btGImpactMeshShape objects + */ + + + void gimpact_vs_gimpact(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1); + + void gimpact_vs_shape(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btCollisionShape * shape1,bool swapped); + + void gimpact_vs_compoundshape(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btCompoundShape * shape1,bool swapped); + + void gimpact_vs_concave( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btConcaveShape * shape1,bool swapped); + + + + + /// Accessor/Mutator pairs for Part and triangleID + void setFace0(int value) + { + m_triface0 = value; + } + int getFace0() + { + return m_triface0; + } + void setFace1(int value) + { + m_triface1 = value; + } + int getFace1() + { + return m_triface1; + } + void setPart0(int value) + { + m_part0 = value; + } + int getPart0() + { + return m_part0; + } + void setPart1(int value) + { + m_part1 = value; + } + int getPart1() + { + return m_part1; + } + +}; + + +//algorithm details +//#define BULLET_TRIANGLE_COLLISION 1 +#define GIMPACT_VS_PLANE_COLLISION 1 + + + +#endif //BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h new file mode 100644 index 00000000000..0a10f3cdb7b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h @@ -0,0 +1,60 @@ +/*! \file btGImpactMassUtil.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 GIMPACT_MASS_UTIL_H +#define GIMPACT_MASS_UTIL_H + +#include "LinearMath/btTransform.h" + + + +SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed( + const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform) +{ + btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); + + btScalar x2 = transform.getOrigin()[0]; + x2*= x2; + btScalar y2 = transform.getOrigin()[1]; + y2*= y2; + btScalar z2 = transform.getOrigin()[2]; + z2*= z2; + + btScalar ix = rotatedTensor[0][0]*(y2+z2); + btScalar iy = rotatedTensor[1][1]*(x2+z2); + btScalar iz = rotatedTensor[2][2]*(x2+y2); + + return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz); +} + +SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass) +{ + btScalar x2 = point[0]*point[0]; + btScalar y2 = point[1]*point[1]; + btScalar z2 = point[2]*point[2]; + return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2)); +} + + +#endif //GIMPACT_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp new file mode 100644 index 00000000000..0b1ef6acd83 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -0,0 +1,529 @@ +/*! \file gim_box_set.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btGImpactQuantizedBvh.h" +#include "LinearMath/btQuickprof.h" + +#ifdef TRI_COLLISION_PROFILING +btClock g_q_tree_clock; + + +float g_q_accum_tree_collision_time = 0; +int g_q_count_traversing = 0; + + +void bt_begin_gim02_q_tree_time() +{ + g_q_tree_clock.reset(); +} + +void bt_end_gim02_q_tree_time() +{ + g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds(); + g_q_count_traversing++; +} + + +//! Gets the average time in miliseconds of tree collisions +float btGImpactQuantizedBvh::getAverageTreeCollisionTime() +{ + if(g_q_count_traversing == 0) return 0; + + float avgtime = g_q_accum_tree_collision_time; + avgtime /= (float)g_q_count_traversing; + + g_q_accum_tree_collision_time = 0; + g_q_count_traversing = 0; + return avgtime; + +// float avgtime = g_q_count_traversing; +// g_q_count_traversing = 0; +// return avgtime; + +} + +#endif //TRI_COLLISION_PROFILING + +/////////////////////// btQuantizedBvhTree ///////////////////////////////// + +void btQuantizedBvhTree::calc_quantization( + BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) +{ + //calc globa box + btAABB global_bound; + global_bound.invalidate(); + + for (int i=0;i<primitive_boxes.size() ;i++ ) + { + global_bound.merge(primitive_boxes[i].m_bound); + } + + bt_calc_quantization_parameters( + m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin); + +} + + + +int btQuantizedBvhTree::_calc_splitting_axis( + BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +{ + + int i; + + 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 = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means+=center; + } + means *= (btScalar(1.)/(btScalar)numIndices); + + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + btVector3 diff2 = center-means; + diff2 = diff2 * diff2; + variance += diff2; + } + variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + + return variance.maxAxis(); +} + + +int btQuantizedBvhTree::_sort_and_calc_splitting_index( + BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + int endIndex, int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + + // average of centers + btScalar splitValue = 0.0f; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means+=center; + } + 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 = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + if (center[splitAxis] > splitValue) + { + //swap + primitive_boxes.swap(i,splitIndex); + //swapLeafNodes(i,splitIndex); + splitIndex++; + } + } + + //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; + +} + + +void btQuantizedBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +{ + int curIndex = m_num_nodes; + m_num_nodes++; + + btAssert((endIndex-startIndex)>0); + + if ((endIndex-startIndex)==1) + { + //We have a leaf node + setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); + + return; + } + //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + + //split axis + int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + + splitIndex = _sort_and_calc_splitting_index( + primitive_boxes,startIndex,endIndex, + splitIndex//split axis + ); + + + //calc this node bounding box + + btAABB node_bound; + node_bound.invalidate(); + + for (int i=startIndex;i<endIndex;i++) + { + node_bound.merge(primitive_boxes[i].m_bound); + } + + setNodeBound(curIndex,node_bound); + + + //build left branch + _build_sub_tree(primitive_boxes, startIndex, splitIndex ); + + + //build right branch + _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + + m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); + + +} + +//! stackless build tree +void btQuantizedBvhTree::build_tree( + BT_BVH_DATA_ARRAY & primitive_boxes) +{ + calc_quantization(primitive_boxes); + // initialize node count to 0 + m_num_nodes = 0; + // allocate nodes + m_node_array.resize(primitive_boxes.size()*2); + + _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); +} + +////////////////////////////////////class btGImpactQuantizedBvh + +void btGImpactQuantizedBvh::refit() +{ + int nodecount = getNodeCount(); + while(nodecount--) + { + if(isLeafNode(nodecount)) + { + btAABB leafbox; + m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); + setNodeBound(nodecount,leafbox); + } + else + { + //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //get left bound + btAABB bound; + bound.invalidate(); + + btAABB temp_box; + + int child_node = getLeftNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + child_node = getRightNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + setNodeBound(nodecount,bound); + } + } +} + +//! this rebuild the entire set +void btGImpactQuantizedBvh::buildSet() +{ + //obtain primitive boxes + BT_BVH_DATA_ARRAY primitive_boxes; + primitive_boxes.resize(m_primitive_manager->get_primitive_count()); + + for (int i = 0;i<primitive_boxes.size() ;i++ ) + { + m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; + } + + m_box_tree.build_tree(primitive_boxes); +} + +//! returns the indices of the primitives in the m_primitive_manager +bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + //quantize box + + unsigned short quantizedMin[3]; + unsigned short quantizedMax[3]; + + m_box_tree.quantizePoint(quantizedMin,box.m_min); + m_box_tree.quantizePoint(quantizedMax,box.m_max); + + + while (curIndex < numNodes) + { + + //catch bugs in tree data + + bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData(curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + if(collided_results.size()>0) return true; + return false; +} + + + +//! returns the indices of the primitives in the m_primitive_manager +bool btGImpactQuantizedBvh::rayQuery( + const btVector3 & ray_dir,const btVector3 & ray_origin , + btAlignedObjectArray<int> & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + btAABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData( curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + if(collided_results.size()>0) return true; + return false; +} + + +SIMD_FORCE_INLINE bool _quantized_node_collision( + btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1, + const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + int node0 ,int node1, bool complete_primitive_tests) +{ + btAABB box0; + boxset0->getNodeBound(node0,box0); + btAABB box1; + boxset1->getNodeBound(node1,box1); + + return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); +// box1.appy_transform_trans_cache(trans_cache_1to0); +// return box0.has_collision(box1); + +} + + +//stackless recursive collision routine +static void _find_quantized_collision_pairs_recursive( + btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1, + btPairSet * collision_pairs, + const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + int node0, int node1, bool complete_primitive_tests) +{ + + + + if( _quantized_node_collision( + boxset0,boxset1,trans_cache_1to0, + node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes + + if(boxset0->isLeafNode(node0)) + { + if(boxset1->isLeafNode(node1)) + { + // collision result + collision_pairs->push_pair( + boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + return; + } + else + { + + //collide left recursive + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + node0,boxset1->getLeftNode(node1),false); + + //collide right recursive + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + node0,boxset1->getRightNode(node1),false); + + + } + } + else + { + if(boxset1->isLeafNode(node1)) + { + + //collide left recursive + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getLeftNode(node0),node1,false); + + + //collide right recursive + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getRightNode(node0),node1,false); + + + } + else + { + //collide left0 left1 + + + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); + + //collide left0 right1 + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); + + + //collide right0 left1 + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); + + //collide right0 right1 + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); + + }// else if node1 is not a leaf + }// else if node0 is not a leaf +} + + +void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0, + btGImpactQuantizedBvh * boxset1, const btTransform & trans1, + btPairSet & collision_pairs) +{ + + if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + + BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; + + trans_cache_1to0.calc_from_homogenic(trans0,trans1); + +#ifdef TRI_COLLISION_PROFILING + bt_begin_gim02_q_tree_time(); +#endif //TRI_COLLISION_PROFILING + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + &collision_pairs,trans_cache_1to0,0,0,true); +#ifdef TRI_COLLISION_PROFILING + bt_end_gim02_q_tree_time(); +#endif //TRI_COLLISION_PROFILING + +} + + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h new file mode 100644 index 00000000000..3a4774b849d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -0,0 +1,379 @@ +#ifndef GIM_QUANTIZED_SET_H_INCLUDED +#define GIM_QUANTIZED_SET_H_INCLUDED + +/*! \file btGImpactQuantizedBvh.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btGImpactBvh.h" +#include "btQuantization.h" + + + +/*! \defgroup BOX_TREES + + + +*/ +//! @{ + + +///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) BT_QUANTIZED_BVH_NODE +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrDataIndex; + + BT_QUANTIZED_BVH_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + + SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( + unsigned short * quantizedMin,unsigned short * quantizedMax) const + { + if(m_quantizedAabbMin[0] > quantizedMax[0] || + m_quantizedAabbMax[0] < quantizedMin[0] || + m_quantizedAabbMin[1] > quantizedMax[1] || + m_quantizedAabbMax[1] < quantizedMin[1] || + m_quantizedAabbMin[2] > quantizedMax[2] || + m_quantizedAabbMax[2] < quantizedMin[2]) + { + return false; + } + return true; + } + +}; + + + +class BT_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE> +{ +}; + + + + +//! Basic Box tree structure +class btQuantizedBvhTree +{ +protected: + int m_num_nodes; + BT_QUANTIZED_BVH_NODE_ARRAY m_node_array; + btAABB m_global_bound; + btVector3 m_bvhQuantization; +protected: + void calc_quantization(BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); + + int _sort_and_calc_splitting_index( + BT_BVH_DATA_ARRAY & primitive_boxes, + int startIndex, int endIndex, int splitAxis); + + int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); +public: + btQuantizedBvhTree() + { + m_num_nodes = 0; + } + + //! prototype functions for box tree management + //!@{ + void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes); + + SIMD_FORCE_INLINE void quantizePoint( + unsigned short * quantizedpoint, const btVector3 & point) const + { + bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization); + } + + + SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( + int node_index, + unsigned short * quantizedMin,unsigned short * quantizedMax) const + { + return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax); + } + + SIMD_FORCE_INLINE void clearNodes() + { + m_node_array.clear(); + m_num_nodes = 0; + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_num_nodes; + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_node_array[nodeindex].isLeafNode(); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_node_array[nodeindex].getDataIndex(); + } + + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + { + bound.m_min = bt_unquantize( + m_node_array[nodeindex].m_quantizedAabbMin, + m_global_bound.m_min,m_bvhQuantization); + + bound.m_max = bt_unquantize( + m_node_array[nodeindex].m_quantizedAabbMax, + m_global_bound.m_min,m_bvhQuantization); + } + + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + { + bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMin, + bound.m_min, + m_global_bound.m_min, + m_global_bound.m_max, + m_bvhQuantization); + + bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMax, + bound.m_max, + m_global_bound.m_min, + m_global_bound.m_max, + m_bvhQuantization); + } + + SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const + { + return nodeindex+1; + } + + SIMD_FORCE_INLINE int getRightNode(int nodeindex) const + { + if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2; + return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex(); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_node_array[nodeindex].getEscapeIndex(); + } + + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + { + return &m_node_array[index]; + } + + //!@} +}; + + + +//! Structure for containing Boxes +/*! +This class offers an structure for managing a box tree of primitives. +Requires a Primitive prototype (like btPrimitiveManagerBase ) +*/ +class btGImpactQuantizedBvh +{ +protected: + btQuantizedBvhTree m_box_tree; + btPrimitiveManagerBase * m_primitive_manager; + +protected: + //stackless refit + void refit(); +public: + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactQuantizedBvh() + { + m_primitive_manager = NULL; + } + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btAABB getGlobalBox() const + { + btAABB totalbox; + getNodeBound(0, totalbox); + return totalbox; + } + + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + { + return m_primitive_manager; + } + + +//! node manager prototype functions +///@{ + + //! this attemps to refit the box set. + SIMD_FORCE_INLINE void update() + { + refit(); + } + + //! this rebuild the entire set + void buildSet(); + + //! returns the indices of the primitives in the m_primitive_manager + bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const; + + //! returns the indices of the primitives in the m_primitive_manager + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box, + const btTransform & transform, btAlignedObjectArray<int> & collided_results) const + { + btAABB transbox=box; + transbox.appy_transform(transform); + return boxQuery(transbox,collided_results); + } + + //! returns the indices of the primitives in the m_primitive_manager + bool rayQuery( + const btVector3 & ray_dir,const btVector3 & ray_origin , + btAlignedObjectArray<int> & collided_results) const; + + //! tells if this set has hierarcht + SIMD_FORCE_INLINE bool hasHierarchy() const + { + return true; + } + + //! tells if this set is a trimesh + SIMD_FORCE_INLINE bool isTrimesh() const + { + return m_primitive_manager->is_trimesh(); + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_box_tree.getNodeCount(); + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_box_tree.isLeafNode(nodeindex); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_box_tree.getNodeData(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + { + m_box_tree.getNodeBound(nodeindex, bound); + } + + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + { + m_box_tree.setNodeBound(nodeindex, bound); + } + + + SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const + { + return m_box_tree.getLeftNode(nodeindex); + } + + SIMD_FORCE_INLINE int getRightNode(int nodeindex) const + { + return m_box_tree.getRightNode(nodeindex); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_box_tree.getEscapeNodeIndex(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + { + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + } + + + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + { + return m_box_tree.get_node_pointer(index); + } + +//! @} + + static float getAverageTreeCollisionTime(); + + + static void find_collision(btGImpactQuantizedBvh * boxset1, const btTransform & trans1, + btGImpactQuantizedBvh * boxset2, const btTransform & trans2, + btPairSet & collision_pairs); +}; + + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp new file mode 100644 index 00000000000..da6a4dbfc28 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp @@ -0,0 +1,183 @@ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btGImpactShape.h" +#include "btGImpactMassUtil.h" + + +#define CALC_EXACT_INERTIA 1 + +void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + lockChildShapes(); +#ifdef CALC_EXACT_INERTIA + inertia.setValue(0.f,0.f,0.f); + + int i = this->getNumChildShapes(); + btScalar shapemass = mass/btScalar(i); + + while(i--) + { + btVector3 temp_inertia; + m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia); + if(childrenHasTransform()) + { + inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]); + } + else + { + inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity()); + } + + } + +#else + + // Calc box inertia + + btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * btScalar(0.08333333); + + inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + +#endif + unlockChildShapes(); +} + + + +void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + lockChildShapes(); + + +#ifdef CALC_EXACT_INERTIA + inertia.setValue(0.f,0.f,0.f); + + int i = this->getVertexCount(); + btScalar pointmass = mass/btScalar(i); + + while(i--) + { + btVector3 pointintertia; + this->getVertex(i,pointintertia); + pointintertia = gim_get_point_inertia(pointintertia,pointmass); + inertia+=pointintertia; + } + +#else + + // Calc box inertia + + btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * btScalar(0.08333333); + + inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + +#endif + + unlockChildShapes(); +} + +void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + +#ifdef CALC_EXACT_INERTIA + inertia.setValue(0.f,0.f,0.f); + + int i = this->getMeshPartCount(); + btScalar partmass = mass/btScalar(i); + + while(i--) + { + btVector3 partinertia; + getMeshPart(i)->calculateLocalInertia(partmass,partinertia); + inertia+=partinertia; + } + +#else + + // Calc box inertia + + btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * btScalar(0.08333333); + + inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + +#endif +} + +void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const +{ +} + + +void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + lockChildShapes(); + btAABB box; + box.m_min = aabbMin; + box.m_max = aabbMax; + + btAlignedObjectArray<int> collided; + m_box_set.boxQuery(box,collided); + + if(collided.size()==0) + { + unlockChildShapes(); + return; + } + + int part = (int)getPart(); + btPrimitiveTriangle triangle; + int i = collided.size(); + while(i--) + { + this->getPrimitiveTriangle(collided[i],triangle); + callback->processTriangle(triangle.m_vertices,part,collided[i]); + } + unlockChildShapes(); + +} + +void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + int i = m_mesh_parts.size(); + while(i--) + { + m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax); + } +} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h new file mode 100644 index 00000000000..43f32bea5a1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h @@ -0,0 +1,1109 @@ +/*! \file btGImpactShape.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 GIMPACT_SHAPE_H +#define GIMPACT_SHAPE_H + +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAlignedObjectArray.h" + +#include "btGImpactQuantizedBvh.h" // box tree class + + +//! declare Quantized trees, (you can change to float based trees) +typedef btGImpactQuantizedBvh btGImpactBoxSet; + +enum eGIMPACT_SHAPE_TYPE +{ + CONST_GIMPACT_COMPOUND_SHAPE = 0, + CONST_GIMPACT_TRIMESH_SHAPE_PART, + CONST_GIMPACT_TRIMESH_SHAPE +}; + + +//! Helper class for tetrahedrons +class btTetrahedronShapeEx:public btBU_Simplex1to4 +{ +public: + btTetrahedronShapeEx() + { + m_numVertices = 4; + } + + + SIMD_FORCE_INLINE void setVertices( + const btVector3 & v0,const btVector3 & v1, + const btVector3 & v2,const btVector3 & v3) + { + m_vertices[0] = v0; + m_vertices[1] = v1; + m_vertices[2] = v2; + m_vertices[3] = v3; + recalcLocalAabb(); + } +}; + + +//! Base class for gimpact shapes +class btGImpactShapeInterface : public btConcaveShape +{ +protected: + btAABB m_localAABB; + bool m_needs_update; + btVector3 localScaling; + btGImpactBoxSet m_box_set;// optionally boxset + + //! use this function for perfofm refit in bounding boxes + //! use this function for perfofm refit in bounding boxes + virtual void calcLocalAABB() + { + lockChildShapes(); + if(m_box_set.getNodeCount() == 0) + { + m_box_set.buildSet(); + } + else + { + m_box_set.update(); + } + unlockChildShapes(); + + m_localAABB = m_box_set.getGlobalBox(); + } + + +public: + btGImpactShapeInterface() + { + m_localAABB.invalidate(); + m_needs_update = true; + localScaling.setValue(1.f,1.f,1.f); + } + + + //! performs refit operation + /*! + Updates the entire Box set of this shape. + \pre postUpdate() must be called for attemps to calculating the box set, else this function + will does nothing. + \post if m_needs_update == true, then it calls calcLocalAABB(); + */ + SIMD_FORCE_INLINE void updateBound() + { + if(!m_needs_update) return; + calcLocalAABB(); + m_needs_update = false; + } + + //! If the Bounding box is not updated, then this class attemps to calculate it. + /*! + \post Calls updateBound() for update the box set. + */ + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + btAABB transformedbox = m_localAABB; + transformedbox.appy_transform(t); + aabbMin = transformedbox.m_min; + aabbMax = transformedbox.m_max; + } + + //! Tells to this object that is needed to refit the box set + virtual void postUpdate() + { + m_needs_update = true; + } + + //! Obtains the local box, which is the global calculated box of the total of subshapes + SIMD_FORCE_INLINE const btAABB & getLocalBox() + { + return m_localAABB; + } + + + virtual int getShapeType() const + { + return GIMPACT_SHAPE_PROXYTYPE; + } + + /*! + \post You must call updateBound() for update the box set. + */ + virtual void setLocalScaling(const btVector3& scaling) + { + localScaling = scaling; + postUpdate(); + } + + virtual const btVector3& getLocalScaling() const + { + return localScaling; + } + + + virtual void setMargin(btScalar margin) + { + m_collisionMargin = margin; + int i = getNumChildShapes(); + while(i--) + { + btCollisionShape* child = getChildShape(i); + child->setMargin(margin); + } + + m_needs_update = true; + } + + + //! Subshape member functions + //!@{ + + //! Base method for determinig which kind of GIMPACT shape we get + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() = 0; + + //! gets boxset + SIMD_FORCE_INLINE btGImpactBoxSet * getBoxSet() + { + return &m_box_set; + } + + //! Determines if this class has a hierarchy structure for sorting its primitives + SIMD_FORCE_INLINE bool hasBoxSet() const + { + if(m_box_set.getNodeCount() == 0) return false; + return true; + } + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const = 0; + + + //! Gets the number of children + virtual int getNumChildShapes() const = 0; + + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const = 0; + + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const = 0; + + //! Determines if this shape has tetrahedrons + virtual bool needsRetrieveTetrahedrons() const = 0; + + virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const = 0; + + virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const = 0; + + + + //! call when reading child shapes + virtual void lockChildShapes() const + { + } + + virtual void unlockChildShapes() const + { + } + + //! if this trimesh + SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const + { + getPrimitiveManager()->get_primitive_triangle(index,triangle); + } + + + //! Retrieves the bound from a child + /*! + */ + virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + btAABB child_aabb; + getPrimitiveManager()->get_primitive_box(child_index,child_aabb); + child_aabb.appy_transform(t); + aabbMin = child_aabb.m_min; + aabbMax = child_aabb.m_max; + } + + //! Gets the children + virtual btCollisionShape* getChildShape(int index) = 0; + + + //! Gets the child + virtual const btCollisionShape* getChildShape(int index) const = 0; + + //! Gets the children transform + virtual btTransform getChildTransform(int index) const = 0; + + //! Sets the children transform + /*! + \post You must call updateBound() for update the box set. + */ + virtual void setChildTransform(int index, const btTransform & transform) = 0; + + //!@} + + + //! virtual method for ray collision + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const + { + } + + //! Function for retrieve triangles. + /*! + It gives the triangles in local space + */ + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const + { + } + + //!@} + +}; + + +//! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once +/*! +This class only can manage Convex subshapes +*/ +class btGImpactCompoundShape : public btGImpactShapeInterface +{ +public: + //! compound primitive manager + class CompoundPrimitiveManager:public btPrimitiveManagerBase + { + public: + btGImpactCompoundShape * m_compoundShape; + + + CompoundPrimitiveManager(const CompoundPrimitiveManager& compound) + { + m_compoundShape = compound.m_compoundShape; + } + + CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape) + { + m_compoundShape = compoundShape; + } + + CompoundPrimitiveManager() + { + m_compoundShape = NULL; + } + + virtual bool is_trimesh() const + { + return false; + } + + virtual int get_primitive_count() const + { + return (int )m_compoundShape->getNumChildShapes(); + } + + virtual void get_primitive_box(int prim_index ,btAABB & primbox) const + { + btTransform prim_trans; + if(m_compoundShape->childrenHasTransform()) + { + prim_trans = m_compoundShape->getChildTransform(prim_index); + } + else + { + prim_trans.setIdentity(); + } + const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index); + shape->getAabb(prim_trans,primbox.m_min,primbox.m_max); + } + + virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + { + btAssert(0); + } + + }; + + + +protected: + CompoundPrimitiveManager m_primitive_manager; + btAlignedObjectArray<btTransform> m_childTransforms; + btAlignedObjectArray<btCollisionShape*> m_childShapes; + + +public: + + btGImpactCompoundShape(bool children_has_transform = true) + { + m_primitive_manager.m_compoundShape = this; + m_box_set.setPrimitiveManager(&m_primitive_manager); + } + + virtual ~btGImpactCompoundShape() + { + } + + + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const + { + if(m_childTransforms.size()==0) return false; + return true; + } + + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const + { + return &m_primitive_manager; + } + + //! Obtains the compopund primitive manager + SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager() + { + return &m_primitive_manager; + } + + //! Gets the number of children + virtual int getNumChildShapes() const + { + return m_childShapes.size(); + } + + + //! Use this method for adding children. Only Convex shapes are allowed. + void addChildShape(const btTransform& localTransform,btCollisionShape* shape) + { + btAssert(shape->isConvex()); + m_childTransforms.push_back(localTransform); + m_childShapes.push_back(shape); + } + + //! Use this method for adding children. Only Convex shapes are allowed. + void addChildShape(btCollisionShape* shape) + { + btAssert(shape->isConvex()); + m_childShapes.push_back(shape); + } + + //! Gets the children + virtual btCollisionShape* getChildShape(int index) + { + return m_childShapes[index]; + } + + //! Gets the children + virtual const btCollisionShape* getChildShape(int index) const + { + return m_childShapes[index]; + } + + //! Retrieves the bound from a child + /*! + */ + virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + + if(childrenHasTransform()) + { + m_childShapes[child_index]->getAabb(t*m_childTransforms[child_index],aabbMin,aabbMax); + } + else + { + m_childShapes[child_index]->getAabb(t,aabbMin,aabbMax); + } + } + + + //! Gets the children transform + virtual btTransform getChildTransform(int index) const + { + btAssert(m_childTransforms.size() == m_childShapes.size()); + return m_childTransforms[index]; + } + + //! Sets the children transform + /*! + \post You must call updateBound() for update the box set. + */ + virtual void setChildTransform(int index, const btTransform & transform) + { + btAssert(m_childTransforms.size() == m_childShapes.size()); + m_childTransforms[index] = transform; + postUpdate(); + } + + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const + { + return false; + } + + //! Determines if this shape has tetrahedrons + virtual bool needsRetrieveTetrahedrons() const + { + return false; + } + + + virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + { + btAssert(0); + } + + virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + { + btAssert(0); + } + + + //! Calculates the exact inertia tensor for this shape + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + virtual const char* getName()const + { + return "GImpactCompound"; + } + + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() + { + return CONST_GIMPACT_COMPOUND_SHAPE; + } + +}; + + + +//! This class manages a sub part of a mesh supplied by the btStridingMeshInterface interface. +/*! +- Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShapePart, then you must call updateBound() after creating the mesh +- When making operations with this shape, you must call <b>lock</b> before accessing to the trimesh primitives, and then call <b>unlock</b> +- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices. + +*/ +class btGImpactMeshShapePart : public btGImpactShapeInterface +{ +public: + //! Trimesh primitive manager + /*! + Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism + */ + class TrimeshPrimitiveManager:public btPrimitiveManagerBase + { + public: + btScalar m_margin; + btStridingMeshInterface * m_meshInterface; + btVector3 m_scale; + int m_part; + int m_lock_count; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + TrimeshPrimitiveManager() + { + m_meshInterface = NULL; + m_part = 0; + m_margin = 0.01f; + m_scale = btVector3(1.f,1.f,1.f); + m_lock_count = 0; + vertexbase = 0; + numverts = 0; + stride = 0; + indexbase = 0; + indexstride = 0; + numfaces = 0; + } + + TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager) + { + m_meshInterface = manager.m_meshInterface; + m_part = manager.m_part; + m_margin = manager.m_margin; + m_scale = manager.m_scale; + m_lock_count = 0; + vertexbase = 0; + numverts = 0; + stride = 0; + indexbase = 0; + indexstride = 0; + numfaces = 0; + + } + + TrimeshPrimitiveManager( + btStridingMeshInterface * meshInterface, int part) + { + m_meshInterface = meshInterface; + m_part = part; + m_scale = m_meshInterface->getScaling(); + m_margin = 0.1f; + m_lock_count = 0; + vertexbase = 0; + numverts = 0; + stride = 0; + indexbase = 0; + indexstride = 0; + numfaces = 0; + + } + + + void lock() + { + if(m_lock_count>0) + { + m_lock_count++; + return; + } + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase,numverts, + type, stride,&indexbase, indexstride, numfaces,indicestype,m_part); + + m_lock_count = 1; + } + + void unlock() + { + if(m_lock_count == 0) return; + if(m_lock_count>1) + { + --m_lock_count; + return; + } + m_meshInterface->unLockReadOnlyVertexBase(m_part); + vertexbase = NULL; + m_lock_count = 0; + } + + virtual bool is_trimesh() const + { + return true; + } + + virtual int get_primitive_count() const + { + return (int )numfaces; + } + + SIMD_FORCE_INLINE int get_vertex_count() const + { + return (int )numverts; + } + + SIMD_FORCE_INLINE void get_indices(int face_index,int &i0,int &i1,int &i2) const + { + if(indicestype == PHY_SHORT) + { + short * s_indices = (short *)(indexbase + face_index*indexstride); + i0 = s_indices[0]; + i1 = s_indices[1]; + i2 = s_indices[2]; + } + else + { + int * i_indices = (int *)(indexbase + face_index*indexstride); + i0 = i_indices[0]; + i1 = i_indices[1]; + i2 = i_indices[2]; + } + } + + SIMD_FORCE_INLINE void get_vertex(int vertex_index, btVector3 & vertex) const + { + if(type == PHY_DOUBLE) + { + double * dvertices = (double *)(vertexbase + vertex_index*stride); + vertex[0] = btScalar(dvertices[0]*m_scale[0]); + vertex[1] = btScalar(dvertices[1]*m_scale[1]); + vertex[2] = btScalar(dvertices[2]*m_scale[2]); + } + else + { + float * svertices = (float *)(vertexbase + vertex_index*stride); + vertex[0] = svertices[0]*m_scale[0]; + vertex[1] = svertices[1]*m_scale[1]; + vertex[2] = svertices[2]*m_scale[2]; + } + } + + virtual void get_primitive_box(int prim_index ,btAABB & primbox) const + { + btPrimitiveTriangle triangle; + get_primitive_triangle(prim_index,triangle); + primbox.calc_from_triangle_margin( + triangle.m_vertices[0], + triangle.m_vertices[1],triangle.m_vertices[2],triangle.m_margin); + } + + virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + { + int indices[3]; + get_indices(prim_index,indices[0],indices[1],indices[2]); + get_vertex(indices[0],triangle.m_vertices[0]); + get_vertex(indices[1],triangle.m_vertices[1]); + get_vertex(indices[2],triangle.m_vertices[2]); + triangle.m_margin = m_margin; + } + + SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index,btTriangleShapeEx & triangle) const + { + int indices[3]; + get_indices(prim_index,indices[0],indices[1],indices[2]); + get_vertex(indices[0],triangle.m_vertices1[0]); + get_vertex(indices[1],triangle.m_vertices1[1]); + get_vertex(indices[2],triangle.m_vertices1[2]); + triangle.setMargin(m_margin); + } + + }; + + +protected: + TrimeshPrimitiveManager m_primitive_manager; +public: + + btGImpactMeshShapePart() + { + m_box_set.setPrimitiveManager(&m_primitive_manager); + } + + + btGImpactMeshShapePart(btStridingMeshInterface * meshInterface, int part) + { + m_primitive_manager.m_meshInterface = meshInterface; + m_primitive_manager.m_part = part; + m_box_set.setPrimitiveManager(&m_primitive_manager); + } + + virtual ~btGImpactMeshShapePart() + { + } + + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const + { + return false; + } + + + //! call when reading child shapes + virtual void lockChildShapes() const + { + void * dummy = (void*)(m_box_set.getPrimitiveManager()); + TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy); + dummymanager->lock(); + } + + virtual void unlockChildShapes() const + { + void * dummy = (void*)(m_box_set.getPrimitiveManager()); + TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy); + dummymanager->unlock(); + } + + //! Gets the number of children + virtual int getNumChildShapes() const + { + return m_primitive_manager.get_primitive_count(); + } + + + //! Gets the children + virtual btCollisionShape* getChildShape(int index) + { + btAssert(0); + return NULL; + } + + + + //! Gets the child + virtual const btCollisionShape* getChildShape(int index) const + { + btAssert(0); + return NULL; + } + + //! Gets the children transform + virtual btTransform getChildTransform(int index) const + { + btAssert(0); + return btTransform(); + } + + //! Sets the children transform + /*! + \post You must call updateBound() for update the box set. + */ + virtual void setChildTransform(int index, const btTransform & transform) + { + btAssert(0); + } + + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const + { + return &m_primitive_manager; + } + + SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager() + { + return &m_primitive_manager; + } + + + + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + + + + virtual const char* getName()const + { + return "GImpactMeshShapePart"; + } + + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() + { + return CONST_GIMPACT_TRIMESH_SHAPE_PART; + } + + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const + { + return true; + } + + //! Determines if this shape has tetrahedrons + virtual bool needsRetrieveTetrahedrons() const + { + return false; + } + + virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + { + m_primitive_manager.get_bullet_triangle(prim_index,triangle); + } + + virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + { + btAssert(0); + } + + + + SIMD_FORCE_INLINE int getVertexCount() const + { + return m_primitive_manager.get_vertex_count(); + } + + SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3 & vertex) const + { + m_primitive_manager.get_vertex(vertex_index,vertex); + } + + SIMD_FORCE_INLINE void setMargin(btScalar margin) + { + m_primitive_manager.m_margin = margin; + postUpdate(); + } + + SIMD_FORCE_INLINE btScalar getMargin() const + { + return m_primitive_manager.m_margin; + } + + virtual void setLocalScaling(const btVector3& scaling) + { + m_primitive_manager.m_scale = scaling; + postUpdate(); + } + + virtual const btVector3& getLocalScaling() const + { + return m_primitive_manager.m_scale; + } + + SIMD_FORCE_INLINE int getPart() const + { + return (int)m_primitive_manager.m_part; + } + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; +}; + + +//! This class manages a mesh supplied by the btStridingMeshInterface interface. +/*! +Set of btGImpactMeshShapePart parts +- Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShape, then you must call updateBound() after creating the mesh + +- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices. + +*/ +class btGImpactMeshShape : public btGImpactShapeInterface +{ +protected: + btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts; + void buildMeshParts(btStridingMeshInterface * meshInterface) + { + for (int i=0;i<meshInterface->getNumSubParts() ;++i ) + { + btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,i); + m_mesh_parts.push_back(newpart); + } + } + + //! use this function for perfofm refit in bounding boxes + virtual void calcLocalAABB() + { + m_localAABB.invalidate(); + int i = m_mesh_parts.size(); + while(i--) + { + m_mesh_parts[i]->updateBound(); + m_localAABB.merge(m_mesh_parts[i]->getLocalBox()); + } + } + +public: + btGImpactMeshShape(btStridingMeshInterface * meshInterface) + { + buildMeshParts(meshInterface); + } + + virtual ~btGImpactMeshShape() + { + int i = m_mesh_parts.size(); + while(i--) + { + btGImpactMeshShapePart * part = m_mesh_parts[i]; + delete part; + } + m_mesh_parts.clear(); + } + + + + int getMeshPartCount() const + { + return m_mesh_parts.size(); + } + + btGImpactMeshShapePart * getMeshPart(int index) + { + return m_mesh_parts[index]; + } + + + + const btGImpactMeshShapePart * getMeshPart(int index) const + { + return m_mesh_parts[index]; + } + + + virtual void setLocalScaling(const btVector3& scaling) + { + localScaling = scaling; + + int i = m_mesh_parts.size(); + while(i--) + { + btGImpactMeshShapePart * part = m_mesh_parts[i]; + part->setLocalScaling(scaling); + } + + m_needs_update = true; + } + + virtual void setMargin(btScalar margin) + { + m_collisionMargin = margin; + + int i = m_mesh_parts.size(); + while(i--) + { + btGImpactMeshShapePart * part = m_mesh_parts[i]; + part->setMargin(margin); + } + + m_needs_update = true; + } + + //! Tells to this object that is needed to refit all the meshes + virtual void postUpdate() + { + int i = m_mesh_parts.size(); + while(i--) + { + btGImpactMeshShapePart * part = m_mesh_parts[i]; + part->postUpdate(); + } + + m_needs_update = true; + } + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const + { + btAssert(0); + return NULL; + } + + + //! Gets the number of children + virtual int getNumChildShapes() const + { + btAssert(0); + return 0; + } + + + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const + { + btAssert(0); + return false; + } + + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const + { + btAssert(0); + return false; + } + + //! Determines if this shape has tetrahedrons + virtual bool needsRetrieveTetrahedrons() const + { + btAssert(0); + return false; + } + + virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + { + btAssert(0); + } + + virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + { + btAssert(0); + } + + //! call when reading child shapes + virtual void lockChildShapes() + { + btAssert(0); + } + + virtual void unlockChildShapes() + { + btAssert(0); + } + + + + + //! Retrieves the bound from a child + /*! + */ + virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + btAssert(0); + } + + //! Gets the children + virtual btCollisionShape* getChildShape(int index) + { + btAssert(0); + return NULL; + } + + + //! Gets the child + virtual const btCollisionShape* getChildShape(int index) const + { + btAssert(0); + return NULL; + } + + //! Gets the children transform + virtual btTransform getChildTransform(int index) const + { + btAssert(0); + return btTransform(); + } + + //! Sets the children transform + /*! + \post You must call updateBound() for update the box set. + */ + virtual void setChildTransform(int index, const btTransform & transform) + { + btAssert(0); + } + + + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() + { + return CONST_GIMPACT_TRIMESH_SHAPE; + } + + + virtual const char* getName()const + { + return "GImpactMesh"; + } + + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; + + //! Function for retrieve triangles. + /*! + It gives the triangles in local space + */ + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; +}; + + +#endif //GIMPACT_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp new file mode 100644 index 00000000000..4b4b5e20dd3 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -0,0 +1,283 @@ +/*! \file btGenericPoolAllocator.cpp +\author Francisco Len Nßjera. email projectileman@yahoo.com + +General purpose allocator class +*/ +/* +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 "btGenericPoolAllocator.h" + + + +/// *************** btGenericMemoryPool ******************/////////// + +size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements) +{ + size_t ptr = BT_UINT_MAX; + + if(m_free_nodes_count == 0) return BT_UINT_MAX; + // find an avaliable free node with the correct size + size_t revindex = m_free_nodes_count; + + while(revindex-- && ptr == BT_UINT_MAX) + { + if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements) + { + ptr = revindex; + } + } + if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found + + + revindex = ptr; + ptr = m_free_nodes[revindex]; + // post: ptr contains the node index, and revindex the index in m_free_nodes + + size_t finalsize = m_allocated_sizes[ptr]; + finalsize -= num_elements; + + m_allocated_sizes[ptr] = num_elements; + + // post: finalsize>=0, m_allocated_sizes[ptr] has the requested size + + if(finalsize>0) // preserve free node, there are some free memory + { + m_free_nodes[revindex] = ptr + num_elements; + m_allocated_sizes[ptr + num_elements] = finalsize; + } + else // delete free node + { + // swap with end + m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1]; + m_free_nodes_count--; + } + + return ptr; +} + +size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements) +{ + if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX; + + size_t ptr = m_allocated_count; + + m_allocated_sizes[m_allocated_count] = num_elements; + m_allocated_count+=num_elements; + + return ptr; +} + + +void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) +{ + m_allocated_count = 0; + m_free_nodes_count = 0; + + m_element_size = element_size; + m_max_element_count = element_count; + + + + + m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16); + m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); + m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); + + for (size_t i = 0;i< m_max_element_count;i++ ) + { + m_allocated_sizes[i] = 0; + } +} + +void btGenericMemoryPool::end_pool() +{ + btAlignedFree(m_pool); + btAlignedFree(m_free_nodes); + btAlignedFree(m_allocated_sizes); + m_allocated_count = 0; + m_free_nodes_count = 0; +} + + +//! Allocates memory in pool +/*! +\param size_bytes size in bytes of the buffer +*/ +void * btGenericMemoryPool::allocate(size_t size_bytes) +{ + + size_t module = size_bytes%m_element_size; + size_t element_count = size_bytes/m_element_size; + if(module>0) element_count++; + + size_t alloc_pos = allocate_from_free_nodes(element_count); + // a free node is found + if(alloc_pos != BT_UINT_MAX) + { + return get_element_data(alloc_pos); + } + // allocate directly on pool + alloc_pos = allocate_from_pool(element_count); + + if(alloc_pos == BT_UINT_MAX) return NULL; // not space + return get_element_data(alloc_pos); +} + +bool btGenericMemoryPool::freeMemory(void * pointer) +{ + unsigned char * pointer_pos = (unsigned char *)pointer; + unsigned char * pool_pos = (unsigned char *)m_pool; + // calc offset + if(pointer_pos<pool_pos) return false;//other pool + size_t offset = size_t(pointer_pos - pool_pos); + if(offset>=get_pool_capacity()) return false;// far away + + // find free position + m_free_nodes[m_free_nodes_count] = offset/m_element_size; + m_free_nodes_count++; + return true; +} + + +/// *******************! btGenericPoolAllocator *******************!/// + + +btGenericPoolAllocator::~btGenericPoolAllocator() +{ + // destroy pools + size_t i; + for (i=0;i<m_pool_count;i++) + { + m_pools[i]->end_pool(); + btAlignedFree(m_pools[i]); + } +} + + +// creates a pool +btGenericMemoryPool * btGenericPoolAllocator::push_new_pool() +{ + if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; + + btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16); + + m_pools[m_pool_count] = newptr; + + m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count); + + m_pool_count++; + return newptr; +} + +void * btGenericPoolAllocator::failback_alloc(size_t size_bytes) +{ + + btGenericMemoryPool * pool = NULL; + + + if(size_bytes<=get_pool_capacity()) + { + pool = push_new_pool(); + } + + if(pool==NULL) // failback + { + return btAlignedAlloc(size_bytes,16); + } + + return pool->allocate(size_bytes); +} + +bool btGenericPoolAllocator::failback_free(void * pointer) +{ + btAlignedFree(pointer); + return true; +} + + +//! Allocates memory in pool +/*! +\param size_bytes size in bytes of the buffer +*/ +void * btGenericPoolAllocator::allocate(size_t size_bytes) +{ + void * ptr = NULL; + + size_t i = 0; + while(i<m_pool_count && ptr == NULL) + { + ptr = m_pools[i]->allocate(size_bytes); + ++i; + } + + if(ptr) return ptr; + + return failback_alloc(size_bytes); +} + +bool btGenericPoolAllocator::freeMemory(void * pointer) +{ + bool result = false; + + size_t i = 0; + while(i<m_pool_count && result == false) + { + result = m_pools[i]->freeMemory(pointer); + ++i; + } + + if(result) return true; + + return failback_free(pointer); +} + +/// ************** STANDARD ALLOCATOR ***************************/// + + +#define BT_DEFAULT_POOL_SIZE 32768 +#define BT_DEFAULT_POOL_ELEMENT_SIZE 8 + +// main allocator +class BT_STANDARD_ALLOCATOR: public btGenericPoolAllocator +{ +public: + BT_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE) + { + } +}; + +// global allocator +BT_STANDARD_ALLOCATOR g_main_allocator; + + +void * btPoolAlloc(size_t size) +{ + return g_main_allocator.allocate(size); +} + +void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize) +{ + void * newptr = btPoolAlloc(newsize); + size_t copysize = oldsize<newsize?oldsize:newsize; + memcpy(newptr,ptr,copysize); + btPoolFree(ptr); + return newptr; +} + +void btPoolFree(void *ptr) +{ + g_main_allocator.freeMemory(ptr); +} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h new file mode 100644 index 00000000000..05b79a82516 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h @@ -0,0 +1,168 @@ +/*! \file btGenericPoolAllocator.h +\author Francisco Len Nßjera. email projectileman@yahoo.com + +General purpose allocator class +*/ +/* +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_GENERIC_POOL_ALLOCATOR_H +#define BT_GENERIC_POOL_ALLOCATOR_H + +#include <limits.h> +#include <stdio.h> +#include <memory.h> +#include "LinearMath/btAlignedAllocator.h" + +#define BT_UINT_MAX UINT_MAX +#define BT_DEFAULT_MAX_POOLS 16 + + +//! Generic Pool class +class btGenericMemoryPool +{ +public: + unsigned char * m_pool; //[m_element_size*m_max_element_count]; + size_t * m_free_nodes; //[m_max_element_count];//! free nodes + size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node + size_t m_allocated_count; + size_t m_free_nodes_count; +protected: + size_t m_element_size; + size_t m_max_element_count; + + size_t allocate_from_free_nodes(size_t num_elements); + size_t allocate_from_pool(size_t num_elements); + +public: + + void init_pool(size_t element_size, size_t element_count); + + void end_pool(); + + + btGenericMemoryPool(size_t element_size, size_t element_count) + { + init_pool(element_size, element_count); + } + + ~btGenericMemoryPool() + { + end_pool(); + } + + + inline size_t get_pool_capacity() + { + return m_element_size*m_max_element_count; + } + + inline size_t gem_element_size() + { + return m_element_size; + } + + inline size_t get_max_element_count() + { + return m_max_element_count; + } + + inline size_t get_allocated_count() + { + return m_allocated_count; + } + + inline size_t get_free_positions_count() + { + return m_free_nodes_count; + } + + inline void * get_element_data(size_t element_index) + { + return &m_pool[element_index*m_element_size]; + } + + //! Allocates memory in pool + /*! + \param size_bytes size in bytes of the buffer + */ + void * allocate(size_t size_bytes); + + bool freeMemory(void * pointer); +}; + + + + +//! Generic Allocator with pools +/*! +General purpose Allocator which can create Memory Pools dynamiacally as needed. +*/ +class btGenericPoolAllocator +{ +protected: + size_t m_pool_element_size; + size_t m_pool_element_count; +public: + btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS]; + size_t m_pool_count; + + + inline size_t get_pool_capacity() + { + return m_pool_element_size*m_pool_element_count; + } + + +protected: + // creates a pool + btGenericMemoryPool * push_new_pool(); + + void * failback_alloc(size_t size_bytes); + + bool failback_free(void * pointer); +public: + + btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count) + { + m_pool_count = 0; + m_pool_element_size = pool_element_size; + m_pool_element_count = pool_element_count; + } + + virtual ~btGenericPoolAllocator(); + + //! Allocates memory in pool + /*! + \param size_bytes size in bytes of the buffer + */ + void * allocate(size_t size_bytes); + + bool freeMemory(void * pointer); +}; + + + +/*! \defgroup POOL_MEMORY_FUNCTIONS +standar managed Memory functions. Memory pools are used. +*/ +//! @{ +void * btPoolAlloc(size_t size); +void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize); +void btPoolFree(void *ptr); +//! @} + + +#endif diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h new file mode 100644 index 00000000000..f5800716a96 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h @@ -0,0 +1,216 @@ +#ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED +#define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED + +/*! \file btGeometryOperations.h +*\author Francisco Len Nßjera + +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btBoxCollision.h" + + + +/*! \defgroup GEOMETRIC_OPERATIONS +*/ +//! @{ + + +#define PLANEDIREPSILON 0.0000001f +#define PARALELENORMALS 0.000001f + + +#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number)) + +/// Calc a plane from a triangle edge an a normal. plane is a vec4f +SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 & e2, const btVector3 & normal,btVector4 & plane) +{ + btVector3 planenormal = (e2-e1).cross(normal); + planenormal.normalize(); + plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal)); +} + + + +//***************** SEGMENT and LINE FUNCTIONS **********************************/// + +/*! Finds the closest point(cp) to (v) on a segment (e1,e2) + */ +SIMD_FORCE_INLINE void bt_closest_point_on_segment( + btVector3 & cp, const btVector3 & v, + const btVector3 &e1,const btVector3 &e2) +{ + btVector3 n = e2-e1; + cp = v - e1; + btScalar _scalar = cp.dot(n)/n.dot(n); + if(_scalar <0.0f) + { + cp = e1; + } + else if(_scalar >1.0f) + { + cp = e2; + } + else + { + cp = _scalar*n + e1; + } +} + + +//! line plane collision +/*! +*\return + -0 if the ray never intersects + -1 if the ray collides in front + -2 if the ray collides in back +*/ + +SIMD_FORCE_INLINE int bt_line_plane_collision( + const btVector4 & plane, + const btVector3 & vDir, + const btVector3 & vPoint, + btVector3 & pout, + btScalar &tparam, + btScalar tmin, btScalar tmax) +{ + + btScalar _dotdir = vDir.dot(plane); + + if(btFabs(_dotdir)<PLANEDIREPSILON) + { + tparam = tmax; + return 0; + } + + btScalar _dis = bt_distance_point_plane(plane,vPoint); + char returnvalue = _dis<0.0f? 2:1; + tparam = -_dis/_dotdir; + + if(tparam<tmin) + { + returnvalue = 0; + tparam = tmin; + } + else if(tparam>tmax) + { + returnvalue = 0; + tparam = tmax; + } + pout = tparam*vDir + vPoint; + return returnvalue; +} + + +//! Find closest points on segments +SIMD_FORCE_INLINE void bt_segment_collision( + const btVector3 & vA1, + const btVector3 & vA2, + const btVector3 & vB1, + const btVector3 & vB2, + btVector3 & vPointA, + btVector3 & vPointB) +{ + btVector3 AD = vA2 - vA1; + btVector3 BD = vB2 - vB1; + btVector3 N = AD.cross(BD); + btScalar tp = N.length2(); + + btVector4 _M;//plane + + if(tp<SIMD_EPSILON)//ARE PARALELE + { + //project B over A + bool invert_b_order = false; + _M[0] = vB1.dot(AD); + _M[1] = vB2.dot(AD); + + if(_M[0]>_M[1]) + { + invert_b_order = true; + BT_SWAP_NUMBERS(_M[0],_M[1]); + } + _M[2] = vA1.dot(AD); + _M[3] = vA2.dot(AD); + //mid points + N[0] = (_M[0]+_M[1])*0.5f; + N[1] = (_M[2]+_M[3])*0.5f; + + if(N[0]<N[1]) + { + if(_M[1]<_M[2]) + { + vPointB = invert_b_order?vB1:vB2; + vPointA = vA1; + } + else if(_M[1]<_M[3]) + { + vPointB = invert_b_order?vB1:vB2; + bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2); + } + else + { + vPointA = vA2; + bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2); + } + } + else + { + if(_M[3]<_M[0]) + { + vPointB = invert_b_order?vB2:vB1; + vPointA = vA2; + } + else if(_M[3]<_M[1]) + { + vPointA = vA2; + bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2); + } + else + { + vPointB = invert_b_order?vB1:vB2; + bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2); + } + } + return; + } + + N = N.cross(BD); + _M.setValue(N[0],N[1],N[2],vB1.dot(N)); + + // get point A as the plane collision point + bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1)); + + /*Closest point on segment*/ + vPointB = vPointA - vB1; + tp = vPointB.dot(BD); + tp/= BD.dot(BD); + tp = BT_CLAMP(tp,0.0f,1.0f); + + vPointB = tp*BD + vB1; +} + + + +//! @} + + +#endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h new file mode 100644 index 00000000000..e9e6ad3d805 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h @@ -0,0 +1,92 @@ +#ifndef BT_QUANTIZATION_H_INCLUDED +#define BT_QUANTIZATION_H_INCLUDED + +/*! \file btQuantization.h +*\author Francisco Len Nßjera + +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "LinearMath/btTransform.h" + + + +/*! \defgroup GEOMETRIC_OPERATIONS +*/ +//! @{ + + + +SIMD_FORCE_INLINE void bt_calc_quantization_parameters( + btVector3 & outMinBound, + btVector3 & outMaxBound, + btVector3 & bvhQuantization, + const btVector3& srcMinBound,const btVector3& srcMaxBound, + btScalar quantizationMargin) +{ + //enlarge the AABB to avoid division by zero when initializing the quantization values + btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + outMinBound = srcMinBound - clampValue; + outMaxBound = srcMaxBound + clampValue; + btVector3 aabbSize = outMaxBound - outMinBound; + bvhQuantization = btVector3(btScalar(65535.0), + btScalar(65535.0), + btScalar(65535.0)) / aabbSize; +} + + +SIMD_FORCE_INLINE void bt_quantize_clamp( + unsigned short* out, + const btVector3& point, + const btVector3 & min_bound, + const btVector3 & max_bound, + const btVector3 & bvhQuantization) +{ + + btVector3 clampedPoint(point); + clampedPoint.setMax(min_bound); + clampedPoint.setMin(max_bound); + + btVector3 v = (clampedPoint - min_bound) * 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); +} + + +SIMD_FORCE_INLINE btVector3 bt_unquantize( + const unsigned short* vecIn, + const btVector3 & offset, + const btVector3 & bvhQuantization) +{ + btVector3 vecOut; + vecOut.setValue( + (btScalar)(vecIn[0]) / (bvhQuantization.getX()), + (btScalar)(vecIn[1]) / (bvhQuantization.getY()), + (btScalar)(vecIn[2]) / (bvhQuantization.getZ())); + vecOut += offset; + return vecOut; +} + +//! @} + + +#endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp new file mode 100644 index 00000000000..0267f7e67c7 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp @@ -0,0 +1,218 @@ +/*! \file btGImpactTriangleShape.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btTriangleShapeEx.h" + + + +void BT_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, + btScalar margin, const btVector3 * points, int point_count) +{ + m_point_count = 0; + m_penetration_depth= -1000.0f; + + int point_indices[MAX_TRI_CLIPPING]; + + int _k; + + for ( _k=0;_k<point_count;_k++) + { + btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin; + + if (_dist>=0.0f) + { + if (_dist>m_penetration_depth) + { + m_penetration_depth = _dist; + point_indices[0] = _k; + m_point_count=1; + } + else if ((_dist+SIMD_EPSILON)>=m_penetration_depth) + { + point_indices[m_point_count] = _k; + m_point_count++; + } + } + } + + for ( _k=0;_k<m_point_count;_k++) + { + m_points[_k] = points[point_indices[_k]]; + } +} + +///class btPrimitiveTriangle +bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other) +{ + btScalar total_margin = m_margin + other.m_margin; + // classify points on other triangle + btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin; + + btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin; + + btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin; + + if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + + // classify points on this triangle + dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin; + + dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin; + + dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin; + + if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + + return true; +} + +int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ) +{ + // edge 0 + + btVector3 temp_points[MAX_TRI_CLIPPING]; + + + btVector4 edgeplane; + + get_edge_plane(0,edgeplane); + + + int clipped_count = bt_plane_clip_triangle( + edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points); + + if (clipped_count == 0) return 0; + + btVector3 temp_points1[MAX_TRI_CLIPPING]; + + + // edge 1 + get_edge_plane(1,edgeplane); + + + clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1); + + if (clipped_count == 0) return 0; + + // edge 2 + get_edge_plane(2,edgeplane); + + clipped_count = bt_plane_clip_polygon( + edgeplane,temp_points1,clipped_count,clipped_points); + + return clipped_count; +} + +bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts) +{ + btScalar margin = m_margin + other.m_margin; + + btVector3 clipped_points[MAX_TRI_CLIPPING]; + int clipped_count; + //create planes + // plane v vs U points + + BT_TRIANGLE_CONTACT contacts1; + + contacts1.m_separating_normal = m_plane; + + + clipped_count = clip_triangle(other,clipped_points); + + if (clipped_count == 0 ) + { + return false;//Reject + } + + //find most deep interval face1 + contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count); + if (contacts1.m_point_count == 0) return false; // too far + //Normal pointing to this triangle + contacts1.m_separating_normal *= -1.f; + + + //Clip tri1 by tri2 edges + BT_TRIANGLE_CONTACT contacts2; + contacts2.m_separating_normal = other.m_plane; + + clipped_count = other.clip_triangle(*this,clipped_points); + + if (clipped_count == 0 ) + { + return false;//Reject + } + + //find most deep interval face1 + contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count); + if (contacts2.m_point_count == 0) return false; // too far + + + + + ////check most dir for contacts + if (contacts2.m_penetration_depth<contacts1.m_penetration_depth) + { + contacts.copy_from(contacts2); + } + else + { + contacts.copy_from(contacts1); + } + return true; +} + + + +///class btTriangleShapeEx: public btTriangleShape + +bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other) +{ + btScalar total_margin = getMargin() + other.getMargin(); + + btVector4 plane0; + buildTriPlane(plane0); + btVector4 plane1; + other.buildTriPlane(plane1); + + // classify points on other triangle + btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin; + + btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin; + + btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin; + + if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + + // classify points on this triangle + dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin; + + dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin; + + dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin; + + if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + + return true; +} + + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h new file mode 100644 index 00000000000..bdaa323d564 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -0,0 +1,180 @@ +/*! \file btGImpactShape.h +\author Francisco Len Nßjera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 TRIANGLE_SHAPE_EX_H +#define TRIANGLE_SHAPE_EX_H + +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "btBoxCollision.h" +#include "btClipPolygon.h" +#include "btGeometryOperations.h" + + +#define MAX_TRI_CLIPPING 16 + +//! Structure for collision +struct BT_TRIANGLE_CONTACT +{ + btScalar m_penetration_depth; + int m_point_count; + btVector4 m_separating_normal; + btVector3 m_points[MAX_TRI_CLIPPING]; + + SIMD_FORCE_INLINE void copy_from(const BT_TRIANGLE_CONTACT& other) + { + m_penetration_depth = other.m_penetration_depth; + m_separating_normal = other.m_separating_normal; + m_point_count = other.m_point_count; + int i = m_point_count; + while(i--) + { + m_points[i] = other.m_points[i]; + } + } + + BT_TRIANGLE_CONTACT() + { + } + + BT_TRIANGLE_CONTACT(const BT_TRIANGLE_CONTACT& other) + { + copy_from(other); + } + + //! classify points that are closer + void merge_points(const btVector4 & plane, + btScalar margin, const btVector3 * points, int point_count); + +}; + + + +class btPrimitiveTriangle +{ +public: + btVector3 m_vertices[3]; + btVector4 m_plane; + btScalar m_margin; + btScalar m_dummy; + btPrimitiveTriangle():m_margin(0.01f) + { + + } + + + SIMD_FORCE_INLINE void buildTriPlane() + { + btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]); + normal.normalize(); + m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal)); + } + + //! Test if triangles could collide + bool overlap_test_conservative(const btPrimitiveTriangle& other); + + //! Calcs the plane which is paralele to the edge and perpendicular to the triangle plane + /*! + \pre this triangle must have its plane calculated. + */ + SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane) const + { + const btVector3 & e0 = m_vertices[edge_index]; + const btVector3 & e1 = m_vertices[(edge_index+1)%3]; + bt_edge_plane(e0,e1,m_plane,plane); + } + + void applyTransform(const btTransform& t) + { + m_vertices[0] = t(m_vertices[0]); + m_vertices[1] = t(m_vertices[1]); + m_vertices[2] = t(m_vertices[2]); + } + + //! Clips the triangle against this + /*! + \pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated. + \return the number of clipped points + */ + int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ); + + //! Find collision using the clipping method + /*! + \pre this triangle and other must have their triangles calculated + */ + bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts); +}; + + + +//! Helper class for colliding Bullet Triangle Shapes +/*! +This class implements a better getAabb method than the previous btTriangleShape class +*/ +class btTriangleShapeEx: public btTriangleShape +{ +public: + + btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0)) + { + } + + btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2): btTriangleShape(p0,p1,p2) + { + } + + btTriangleShapeEx(const btTriangleShapeEx & other): btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2]) + { + } + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const + { + btVector3 tv0 = t(m_vertices1[0]); + btVector3 tv1 = t(m_vertices1[1]); + btVector3 tv2 = t(m_vertices1[2]); + + btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin); + aabbMin = trianglebox.m_min; + aabbMax = trianglebox.m_max; + } + + void applyTransform(const btTransform& t) + { + m_vertices1[0] = t(m_vertices1[0]); + m_vertices1[1] = t(m_vertices1[1]); + m_vertices1[2] = t(m_vertices1[2]); + } + + SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const + { + btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]); + normal.normalize(); + plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal)); + } + + bool overlap_test_conservative(const btTriangleShapeEx& other); +}; + + +#endif //TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h new file mode 100644 index 00000000000..89fbb5ab0a5 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h @@ -0,0 +1,332 @@ +#ifndef GIM_ARRAY_H_INCLUDED +#define GIM_ARRAY_H_INCLUDED +/*! \file gim_array.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "gim_memory.h" + +/*! \addtogroup CONTAINERS +\brief +Abstract class for template containers +*/ +//! @{ + +#define GIM_ARRAY_GROW_INCREMENT 2 +#define GIM_ARRAY_GROW_FACTOR 2 + +//! Very simple array container with fast access and simd memory +template<typename T> +class gim_array +{ +public: +//! properties +//!@{ + T *m_data; + GUINT m_size; + GUINT m_allocated_size; +//!@} +//! protected operations +//!@{ + + inline void destroyData() + { + m_allocated_size = 0; + if(m_data==NULL) return; + gim_free(m_data); + m_data = NULL; + } + + inline bool resizeData(GUINT newsize) + { + if(newsize==0) + { + destroyData(); + return true; + } + + if(m_size>0) + { + m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T)); + } + else + { + m_data = (T*)gim_alloc(newsize*sizeof(T)); + } + m_allocated_size = newsize; + return true; + } + + inline bool growingCheck() + { + if(m_allocated_size<=m_size) + { + GUINT requestsize = m_size; + m_size = m_allocated_size; + if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false; + } + return true; + } + +//!@} +//! public operations +//!@{ + inline bool reserve(GUINT size) + { + if(m_allocated_size>=size) return false; + return resizeData(size); + } + + inline void clear_range(GUINT start_range) + { + while(m_size>start_range) + { + m_data[--m_size].~T(); + } + } + + inline void clear() + { + if(m_size==0)return; + clear_range(0); + } + + inline void clear_memory() + { + clear(); + destroyData(); + } + + gim_array() + { + m_data = 0; + m_size = 0; + m_allocated_size = 0; + } + + gim_array(GUINT reservesize) + { + m_data = 0; + m_size = 0; + + m_allocated_size = 0; + reserve(reservesize); + } + + ~gim_array() + { + clear_memory(); + } + + inline GUINT size() const + { + return m_size; + } + + inline GUINT max_size() const + { + return m_allocated_size; + } + + inline T & operator[](size_t i) + { + return m_data[i]; + } + inline const T & operator[](size_t i) const + { + return m_data[i]; + } + + inline T * pointer(){ return m_data;} + inline const T * pointer() const + { return m_data;} + + + inline T * get_pointer_at(GUINT i) + { + return m_data + i; + } + + inline const T * get_pointer_at(GUINT i) const + { + return m_data + i; + } + + inline T & at(GUINT i) + { + return m_data[i]; + } + + inline const T & at(GUINT i) const + { + return m_data[i]; + } + + inline T & front() + { + return *m_data; + } + + inline const T & front() const + { + return *m_data; + } + + inline T & back() + { + return m_data[m_size-1]; + } + + inline const T & back() const + { + return m_data[m_size-1]; + } + + + inline void swap(GUINT i, GUINT j) + { + gim_swap_elements(m_data,i,j); + } + + inline void push_back(const T & obj) + { + this->growingCheck(); + m_data[m_size] = obj; + m_size++; + } + + //!Simply increase the m_size, doesn't call the new element constructor + inline void push_back_mem() + { + this->growingCheck(); + m_size++; + } + + inline void push_back_memcpy(const T & obj) + { + this->growingCheck(); + irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); + m_size++; + } + + inline void pop_back() + { + m_size--; + m_data[m_size].~T(); + } + + //!Simply decrease the m_size, doesn't call the deleted element destructor + inline void pop_back_mem() + { + m_size--; + } + + //! fast erase + inline void erase(GUINT index) + { + if(index<m_size-1) + { + swap(index,m_size-1); + } + pop_back(); + } + + inline void erase_sorted_mem(GUINT index) + { + m_size--; + for(GUINT i = index;i<m_size;i++) + { + gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T)); + } + } + + inline void erase_sorted(GUINT index) + { + m_data[index].~T(); + erase_sorted_mem(index); + } + + inline void insert_mem(GUINT index) + { + this->growingCheck(); + for(GUINT i = m_size;i>index;i--) + { + gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T)); + } + m_size++; + } + + inline void insert(const T & obj,GUINT index) + { + insert_mem(index); + m_data[index] = obj; + } + + inline void resize(GUINT size, bool call_constructor = true) + { + + if(size>m_size) + { + reserve(size); + if(call_constructor) + { + T obj; + while(m_size<size) + { + m_data[m_size] = obj; + m_size++; + } + } + else + { + m_size = size; + } + } + else if(size<m_size) + { + if(call_constructor) clear_range(size); + m_size = size; + } + } + + inline void refit() + { + resizeData(m_size); + } +//!@} +}; + + +//! @} + + + +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h new file mode 100644 index 00000000000..d4cbce04b5e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -0,0 +1,547 @@ +#ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED +#define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED + +/*! \file gim_basic_geometry_operations.h +*\author Francisco Len Nßjera +type independant geometry routines + +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + + +#include "gim_linear_math.h" + + + +/*! \defgroup GEOMETRIC_OPERATIONS +*/ +//! @{ + + +#define PLANEDIREPSILON 0.0000001f +#define PARALELENORMALS 0.000001f + + +#define TRIANGLE_NORMAL(v1,v2,v3,n)\ +{\ + vec3f _dif1,_dif2;\ + VEC_DIFF(_dif1,v2,v1);\ + VEC_DIFF(_dif2,v3,v1);\ + VEC_CROSS(n,_dif1,_dif2);\ + VEC_NORMALIZE(n);\ +}\ + +#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\ + vec3f _dif1,_dif2; \ + VEC_DIFF(_dif1,v2,v1); \ + VEC_DIFF(_dif2,v3,v1); \ + VEC_CROSS(n,_dif1,_dif2); \ +}\ + +/// plane is a vec4f +#define TRIANGLE_PLANE(v1,v2,v3,plane) {\ + TRIANGLE_NORMAL(v1,v2,v3,plane);\ + plane[3] = VEC_DOT(v1,plane);\ +}\ + +/// plane is a vec4f +#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\ + TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\ + plane[3] = VEC_DOT(v1,plane);\ +}\ + +/// Calc a plane from an edge an a normal. plane is a vec4f +#define EDGE_PLANE(e1,e2,n,plane) {\ + vec3f _dif; \ + VEC_DIFF(_dif,e2,e1); \ + VEC_CROSS(plane,_dif,n); \ + VEC_NORMALIZE(plane); \ + plane[3] = VEC_DOT(e1,plane);\ +}\ + +#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3]) + +#define PROJECT_POINT_PLANE(point,plane,projected) {\ + GREAL _dis;\ + _dis = DISTANCE_PLANE_POINT(plane,point);\ + VEC_SCALE(projected,-_dis,plane);\ + VEC_SUM(projected,projected,point); \ +}\ + +//! Verifies if a point is in the plane hull +template<typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE bool POINT_IN_HULL( + const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count) +{ + GREAL _dis; + for (GUINT _i = 0;_i< plane_count;++_i) + { + _dis = DISTANCE_PLANE_POINT(planes[_i],point); + if(_dis>0.0f) return false; + } + return true; +} + +template<typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT( + const CLASS_POINT& s1, + const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped) +{ + GREAL _dis1,_dis2; + _dis1 = DISTANCE_PLANE_POINT(plane,s1); + VEC_DIFF(clipped,s2,s1); + _dis2 = VEC_DOT(clipped,plane); + VEC_SCALE(clipped,-_dis1/_dis2,clipped); + VEC_SUM(clipped,clipped,s1); +} + +enum ePLANE_INTERSECTION_TYPE +{ + G_BACK_PLANE = 0, + G_COLLIDE_PLANE, + G_FRONT_PLANE +}; + +enum eLINE_PLANE_INTERSECTION_TYPE +{ + G_FRONT_PLANE_S1 = 0, + G_FRONT_PLANE_S2, + G_BACK_PLANE_S1, + G_BACK_PLANE_S2, + G_COLLIDE_PLANE_S1, + G_COLLIDE_PLANE_S2 +}; + +//! Confirms if the plane intersect the edge or nor +/*! +intersection type must have the following values +<ul> +<li> 0 : Segment in front of plane, s1 closest +<li> 1 : Segment in front of plane, s2 closest +<li> 2 : Segment in back of plane, s1 closest +<li> 3 : Segment in back of plane, s2 closest +<li> 4 : Segment collides plane, s1 in back +<li> 5 : Segment collides plane, s2 in back +</ul> +*/ + +template<typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2( + const CLASS_POINT& s1, + const CLASS_POINT &s2, + const CLASS_PLANE &plane,CLASS_POINT &clipped) +{ + GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1); + GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2); + if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON) + { + if(_dis1<_dis2) return G_FRONT_PLANE_S1; + return G_FRONT_PLANE_S2; + } + else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON) + { + if(_dis1>_dis2) return G_BACK_PLANE_S1; + return G_BACK_PLANE_S2; + } + + VEC_DIFF(clipped,s2,s1); + _dis2 = VEC_DOT(clipped,plane); + VEC_SCALE(clipped,-_dis1/_dis2,clipped); + VEC_SUM(clipped,clipped,s1); + if(_dis1<_dis2) return G_COLLIDE_PLANE_S1; + return G_COLLIDE_PLANE_S2; +} + +//! Confirms if the plane intersect the edge or not +/*! +clipped1 and clipped2 are the vertices behind the plane. +clipped1 is the closest + +intersection_type must have the following values +<ul> +<li> 0 : Segment in front of plane, s1 closest +<li> 1 : Segment in front of plane, s2 closest +<li> 2 : Segment in back of plane, s1 closest +<li> 3 : Segment in back of plane, s2 closest +<li> 4 : Segment collides plane, s1 in back +<li> 5 : Segment collides plane, s2 in back +</ul> +*/ +template<typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( + const CLASS_POINT& s1, + const CLASS_POINT &s2, + const CLASS_PLANE &plane, + CLASS_POINT &clipped1,CLASS_POINT &clipped2) +{ + eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1); + switch(intersection_type) + { + case G_FRONT_PLANE_S1: + VEC_COPY(clipped1,s1); + VEC_COPY(clipped2,s2); + break; + case G_FRONT_PLANE_S2: + VEC_COPY(clipped1,s2); + VEC_COPY(clipped2,s1); + break; + case G_BACK_PLANE_S1: + VEC_COPY(clipped1,s1); + VEC_COPY(clipped2,s2); + break; + case G_BACK_PLANE_S2: + VEC_COPY(clipped1,s2); + VEC_COPY(clipped2,s1); + break; + case G_COLLIDE_PLANE_S1: + VEC_COPY(clipped2,s1); + break; + case G_COLLIDE_PLANE_S2: + VEC_COPY(clipped2,s2); + break; + } + return intersection_type; +} + + +//! Finds the 2 smallest cartesian coordinates of a plane normal +#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1) + +//! Ray plane collision in one way +/*! +Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/> +It uses the PLANEDIREPSILON constant. +*/ +template<typename T,typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION( + const CLASS_PLANE & plane, + const CLASS_POINT & vDir, + const CLASS_POINT & vPoint, + CLASS_POINT & pout,T &tparam) +{ + GREAL _dis,_dotdir; + _dotdir = VEC_DOT(plane,vDir); + if(_dotdir<PLANEDIREPSILON) + { + return false; + } + _dis = DISTANCE_PLANE_POINT(plane,vPoint); + tparam = -_dis/_dotdir; + VEC_SCALE(pout,tparam,vDir); + VEC_SUM(pout,vPoint,pout); + return true; +} + +//! line collision +/*! +*\return + -0 if the ray never intersects + -1 if the ray collides in front + -2 if the ray collides in back +*/ +template<typename T,typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( + const CLASS_PLANE & plane, + const CLASS_POINT & vDir, + const CLASS_POINT & vPoint, + CLASS_POINT & pout, + T &tparam, + T tmin, T tmax) +{ + GREAL _dis,_dotdir; + _dotdir = VEC_DOT(plane,vDir); + if(btFabs(_dotdir)<PLANEDIREPSILON) + { + tparam = tmax; + return 0; + } + _dis = DISTANCE_PLANE_POINT(plane,vPoint); + char returnvalue = _dis<0.0f?2:1; + tparam = -_dis/_dotdir; + + if(tparam<tmin) + { + returnvalue = 0; + tparam = tmin; + } + else if(tparam>tmax) + { + returnvalue = 0; + tparam = tmax; + } + + VEC_SCALE(pout,tparam,vDir); + VEC_SUM(pout,vPoint,pout); + return returnvalue; +} + +/*! \brief Returns the Ray on which 2 planes intersect if they do. + Written by Rodrigo Hernandez on ODE convex collision + + \param p1 Plane 1 + \param p2 Plane 2 + \param p Contains the origin of the ray upon returning if planes intersect + \param d Contains the direction of the ray upon returning if planes intersect + \return true if the planes intersect, 0 if paralell. + +*/ +template<typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE bool INTERSECT_PLANES( + const CLASS_PLANE &p1, + const CLASS_PLANE &p2, + CLASS_POINT &p, + CLASS_POINT &d) +{ + VEC_CROSS(d,p1,p2); + GREAL denom = VEC_DOT(d, d); + if(GIM_IS_ZERO(denom)) return false; + vec3f _n; + _n[0]=p1[3]*p2[0] - p2[3]*p1[0]; + _n[1]=p1[3]*p2[1] - p2[3]*p1[1]; + _n[2]=p1[3]*p2[2] - p2[3]*p1[2]; + VEC_CROSS(p,_n,d); + p[0]/=denom; + p[1]/=denom; + p[2]/=denom; + return true; +} + +//***************** SEGMENT and LINE FUNCTIONS **********************************/// + +/*! Finds the closest point(cp) to (v) on a segment (e1,e2) + */ +template<typename CLASS_POINT> +SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( + CLASS_POINT & cp, const CLASS_POINT & v, + const CLASS_POINT &e1,const CLASS_POINT &e2) +{ + vec3f _n; + VEC_DIFF(_n,e2,e1); + VEC_DIFF(cp,v,e1); + GREAL _scalar = VEC_DOT(cp, _n); + _scalar/= VEC_DOT(_n, _n); + if(_scalar <0.0f) + { + VEC_COPY(cp,e1); + } + else if(_scalar >1.0f) + { + VEC_COPY(cp,e2); + } + else + { + VEC_SCALE(cp,_scalar,_n); + VEC_SUM(cp,cp,e1); + } +} + + +/*! \brief Finds the line params where these lines intersect. + +\param dir1 Direction of line 1 +\param point1 Point of line 1 +\param dir2 Direction of line 2 +\param point2 Point of line 2 +\param t1 Result Parameter for line 1 +\param t2 Result Parameter for line 2 +\param dointersect 0 if the lines won't intersect, else 1 + +*/ +template<typename T,typename CLASS_POINT> +SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS( + const CLASS_POINT & dir1, + CLASS_POINT & point1, + const CLASS_POINT & dir2, + CLASS_POINT & point2, + T& t1,T& t2) +{ + GREAL det; + GREAL e1e1 = VEC_DOT(dir1,dir1); + GREAL e1e2 = VEC_DOT(dir1,dir2); + GREAL e2e2 = VEC_DOT(dir2,dir2); + vec3f p1p2; + VEC_DIFF(p1p2,point1,point2); + GREAL p1p2e1 = VEC_DOT(p1p2,dir1); + GREAL p1p2e2 = VEC_DOT(p1p2,dir2); + det = e1e2*e1e2 - e1e1*e2e2; + if(GIM_IS_ZERO(det)) return false; + t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det; + t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det; + return true; +} + +//! Find closest points on segments +template<typename CLASS_POINT> +SIMD_FORCE_INLINE void SEGMENT_COLLISION( + const CLASS_POINT & vA1, + const CLASS_POINT & vA2, + const CLASS_POINT & vB1, + const CLASS_POINT & vB2, + CLASS_POINT & vPointA, + CLASS_POINT & vPointB) +{ + CLASS_POINT _AD,_BD,_N; + vec4f _M;//plane + VEC_DIFF(_AD,vA2,vA1); + VEC_DIFF(_BD,vB2,vB1); + VEC_CROSS(_N,_AD,_BD); + GREAL _tp = VEC_DOT(_N,_N); + if(_tp<G_EPSILON)//ARE PARALELE + { + //project B over A + bool invert_b_order = false; + _M[0] = VEC_DOT(vB1,_AD); + _M[1] = VEC_DOT(vB2,_AD); + if(_M[0]>_M[1]) + { + invert_b_order = true; + GIM_SWAP_NUMBERS(_M[0],_M[1]); + } + _M[2] = VEC_DOT(vA1,_AD); + _M[3] = VEC_DOT(vA2,_AD); + //mid points + _N[0] = (_M[0]+_M[1])*0.5f; + _N[1] = (_M[2]+_M[3])*0.5f; + + if(_N[0]<_N[1]) + { + if(_M[1]<_M[2]) + { + vPointB = invert_b_order?vB1:vB2; + vPointA = vA1; + } + else if(_M[1]<_M[3]) + { + vPointB = invert_b_order?vB1:vB2; + CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); + } + else + { + vPointA = vA2; + CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); + } + } + else + { + if(_M[3]<_M[0]) + { + vPointB = invert_b_order?vB2:vB1; + vPointA = vA2; + } + else if(_M[3]<_M[1]) + { + vPointA = vA2; + CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); + } + else + { + vPointB = invert_b_order?vB1:vB2; + CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); + } + } + return; + } + + + VEC_CROSS(_M,_N,_BD); + _M[3] = VEC_DOT(_M,vB1); + + LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1)); + /*Closest point on segment*/ + VEC_DIFF(vPointB,vPointA,vB1); + _tp = VEC_DOT(vPointB, _BD); + _tp/= VEC_DOT(_BD, _BD); + _tp = GIM_CLAMP(_tp,0.0f,1.0f); + VEC_SCALE(vPointB,_tp,_BD); + VEC_SUM(vPointB,vPointB,vB1); +} + + + + +//! Line box intersection in one dimension +/*! + +*\param pos Position of the ray +*\param dir Projection of the Direction of the ray +*\param bmin Minimum bound of the box +*\param bmax Maximum bound of the box +*\param tfirst the minimum projection. Assign to 0 at first. +*\param tlast the maximum projection. Assign to INFINITY at first. +*\return true if there is an intersection. +*/ +template<typename T> +SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast) +{ + if(GIM_IS_ZERO(dir)) + { + return !(pos < bmin || pos > bmax); + } + GREAL a0 = (bmin - pos) / dir; + GREAL a1 = (bmax - pos) / dir; + if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1); + tfirst = GIM_MAX(a0, tfirst); + tlast = GIM_MIN(a1, tlast); + if (tlast < tfirst) return false; + return true; +} + + +//! Sorts 3 componets +template<typename T> +SIMD_FORCE_INLINE void SORT_3_INDICES( + const T * values, + GUINT * order_indices) +{ + //get minimum + order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2); + + //get second and third + GUINT i0 = (order_indices[0] + 1)%3; + GUINT i1 = (i0 + 1)%3; + + if(values[i0] < values[i1]) + { + order_indices[1] = i0; + order_indices[2] = i1; + } + else + { + order_indices[1] = i1; + order_indices[2] = i0; + } +} + + + +//! @} + + +#endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h new file mode 100644 index 00000000000..7784aeff140 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h @@ -0,0 +1,129 @@ +#ifndef GIM_BITSET_H_INCLUDED +#define GIM_BITSET_H_INCLUDED +/*! \file gim_bitset.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "gim_array.h" + +/*! \addtogroup CONTAINERS +\brief +Abstract class for template containers +*/ +//! @{ + +#define GUINT_BIT_COUNT 32 +#define GUINT_EXPONENT 5 + +class gim_bitset +{ +public: + gim_array<GUINT> m_container; + + gim_bitset() + { + + } + + gim_bitset(GUINT bits_count) + { + resize(bits_count); + } + + ~gim_bitset() + { + } + + inline bool resize(GUINT newsize) + { + GUINT oldsize = m_container.size(); + m_container.resize(newsize/GUINT_BIT_COUNT + 1,false); + while(oldsize<m_container.size()) + { + m_container[oldsize] = 0; + } + return true; + } + + inline GUINT size() + { + return m_container.size()*GUINT_BIT_COUNT; + } + + inline void set_all() + { + for(GUINT i = 0;i<m_container.size();++i) + { + m_container[i] = 0xffffffff; + } + } + + inline void clear_all() + { + for(GUINT i = 0;i<m_container.size();++i) + { + m_container[i] = 0; + } + } + + inline void set(GUINT bit_index) + { + if(bit_index>=size()) + { + resize(bit_index); + } + m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1))); + } + + ///Return 0 or 1 + inline char get(GUINT bit_index) + { + if(bit_index>=size()) + { + return 0; + } + char value = m_container[bit_index >> GUINT_EXPONENT] & + (1 << (bit_index & (GUINT_BIT_COUNT-1))); + return value; + } + + inline void clear(GUINT bit_index) + { + m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1))); + } +}; + + +//! @} + + + +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h new file mode 100644 index 00000000000..c9bb83e3653 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h @@ -0,0 +1,594 @@ +#ifndef GIM_BOX_COLLISION_H_INCLUDED +#define GIM_BOX_COLLISION_H_INCLUDED + +/*! \file gim_box_collision.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ +#include "gim_basic_geometry_operations.h" +#include "LinearMath/btTransform.h" + +/*! \defgroup BOUND_AABB_OPERATIONS +*/ +//! @{ + + +//SIMD_FORCE_INLINE bool test_cross_edge_box( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, const btVector3 & extend, +// int dir_index0, +// int dir_index1 +// int component_index0, +// int component_index1) +//{ +// // dir coords are -z and y +// +// const btScalar dir0 = -edge[dir_index0]; +// const btScalar dir1 = edge[dir_index1]; +// btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1; +// btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1; +// //find minmax +// if(pmin>pmax) +// { +// GIM_SWAP_NUMBERS(pmin,pmax); +// } +// //find extends +// const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] + +// extend[component_index1] * absolute_edge[dir_index1]; +// +// if(pmin>rad || -rad>pmax) return false; +// return true; +//} +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2); +//} +// +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0); +//} +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1); +//} + +#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\ +{\ + const btScalar dir0 = -edge[i_dir_0];\ + const btScalar dir1 = edge[i_dir_1];\ + btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\ + btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\ + if(pmin>pmax)\ + {\ + GIM_SWAP_NUMBERS(pmin,pmax); \ + }\ + const btScalar abs_dir0 = absolute_edge[i_dir_0];\ + const btScalar abs_dir1 = absolute_edge[i_dir_1];\ + const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\ + if(pmin>rad || -rad>pmax) return false;\ +}\ + + +#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ +{\ + TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\ +}\ + +#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ +{\ + TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\ +}\ + +#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ +{\ + TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\ +}\ + + + +//! Class for transforming a model1 to the space of model0 +class GIM_BOX_BOX_TRANSFORM_CACHE +{ +public: + btVector3 m_T1to0;//!< Transforms translation of model1 to model 0 + btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1 + btMatrix3x3 m_AR;//!< Absolute value of m_R1to0 + + SIMD_FORCE_INLINE void calc_absolute_matrix() + { + static const btVector3 vepsi(1e-6f,1e-6f,1e-6f); + m_AR[0] = vepsi + m_R1to0[0].absolute(); + m_AR[1] = vepsi + m_R1to0[1].absolute(); + m_AR[2] = vepsi + m_R1to0[2].absolute(); + } + + GIM_BOX_BOX_TRANSFORM_CACHE() + { + } + + + GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0) + { + COPY_MATRIX_3X3(m_R1to0,trans1_to_0) + MAT_GET_TRANSLATION(trans1_to_0,m_T1to0) + calc_absolute_matrix(); + } + + //! Calc the transformation relative 1 to 0. Inverts matrics by transposing + SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1) + { + + m_R1to0 = trans0.getBasis().transpose(); + m_T1to0 = m_R1to0 * (-trans0.getOrigin()); + + m_T1to0 += m_R1to0*trans1.getOrigin(); + m_R1to0 *= trans1.getBasis(); + + calc_absolute_matrix(); + } + + //! Calcs the full invertion of the matrices. Useful for scaling matrices + SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1) + { + m_R1to0 = trans0.getBasis().inverse(); + m_T1to0 = m_R1to0 * (-trans0.getOrigin()); + + m_T1to0 += m_R1to0*trans1.getOrigin(); + m_R1to0 *= trans1.getBasis(); + + calc_absolute_matrix(); + } + + SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) + { + return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(), + m_R1to0[1].dot(point) + m_T1to0.y(), + m_R1to0[2].dot(point) + m_T1to0.z()); + } +}; + + +#define BOX_PLANE_EPSILON 0.000001f + +//! Axis aligned box +class GIM_AABB +{ +public: + btVector3 m_min; + btVector3 m_max; + + GIM_AABB() + {} + + + GIM_AABB(const btVector3 & V1, + const btVector3 & V2, + const btVector3 & V3) + { + m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + } + + GIM_AABB(const btVector3 & V1, + const btVector3 & V2, + const btVector3 & V3, + GREAL margin) + { + m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + GIM_AABB(const GIM_AABB &other): + m_min(other.m_min),m_max(other.m_max) + { + } + + GIM_AABB(const GIM_AABB &other,btScalar margin ): + m_min(other.m_min),m_max(other.m_max) + { + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + SIMD_FORCE_INLINE void invalidate() + { + m_min[0] = G_REAL_INFINITY; + m_min[1] = G_REAL_INFINITY; + m_min[2] = G_REAL_INFINITY; + m_max[0] = -G_REAL_INFINITY; + m_max[1] = -G_REAL_INFINITY; + m_max[2] = -G_REAL_INFINITY; + } + + SIMD_FORCE_INLINE void increment_margin(btScalar margin) + { + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin) + { + m_min[0] = other.m_min[0] - margin; + m_min[1] = other.m_min[1] - margin; + m_min[2] = other.m_min[2] - margin; + + m_max[0] = other.m_max[0] + margin; + m_max[1] = other.m_max[1] + margin; + m_max[2] = other.m_max[2] + margin; + } + + template<typename CLASS_POINT> + SIMD_FORCE_INLINE void calc_from_triangle( + const CLASS_POINT & V1, + const CLASS_POINT & V2, + const CLASS_POINT & V3) + { + m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + } + + template<typename CLASS_POINT> + SIMD_FORCE_INLINE void calc_from_triangle_margin( + const CLASS_POINT & V1, + const CLASS_POINT & V2, + const CLASS_POINT & V3, btScalar margin) + { + m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); + m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); + m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + + m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); + m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); + m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + //! Apply a transform to an AABB + SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extends = m_max - center; + // Compute new center + center = trans(center); + + btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()), + extends.dot(trans.getBasis().getRow(1).absolute()), + extends.dot(trans.getBasis().getRow(2).absolute())); + + m_min = center - textends; + m_max = center + textends; + } + + //! Merges a Box + SIMD_FORCE_INLINE void merge(const GIM_AABB & box) + { + m_min[0] = GIM_MIN(m_min[0],box.m_min[0]); + m_min[1] = GIM_MIN(m_min[1],box.m_min[1]); + m_min[2] = GIM_MIN(m_min[2],box.m_min[2]); + + m_max[0] = GIM_MAX(m_max[0],box.m_max[0]); + m_max[1] = GIM_MAX(m_max[1],box.m_max[1]); + m_max[2] = GIM_MAX(m_max[2],box.m_max[2]); + } + + //! Merges a point + template<typename CLASS_POINT> + SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point) + { + m_min[0] = GIM_MIN(m_min[0],point[0]); + m_min[1] = GIM_MIN(m_min[1],point[1]); + m_min[2] = GIM_MIN(m_min[2],point[2]); + + m_max[0] = GIM_MAX(m_max[0],point[0]); + m_max[1] = GIM_MAX(m_max[1],point[1]); + m_max[2] = GIM_MAX(m_max[2],point[2]); + } + + //! Gets the extend and center + SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const + { + center = (m_max+m_min)*0.5f; + extend = m_max - center; + } + + //! Finds the intersecting box between this box and the other. + SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection) const + { + intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]); + intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]); + intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]); + + intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]); + intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]); + intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]); + } + + + SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const + { + if(m_min[0] > other.m_max[0] || + m_max[0] < other.m_min[0] || + m_min[1] > other.m_max[1] || + m_max[1] < other.m_min[1] || + m_min[2] > other.m_max[2] || + m_max[2] < other.m_min[2]) + { + return false; + } + return true; + } + + /*! \brief Finds the Ray intersection parameter. + \param aabb Aligned box + \param vorigin A vec3f with the origin of the ray + \param vdir A vec3f with the direction of the ray + */ + SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir) + { + btVector3 extents,center; + this->get_center_extend(center,extents);; + + btScalar Dx = vorigin[0] - center[0]; + if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false; + btScalar Dy = vorigin[1] - center[1]; + if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false; + btScalar Dz = vorigin[2] - center[2]; + if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false; + + + btScalar f = vdir[1] * Dz - vdir[2] * Dy; + if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false; + f = vdir[2] * Dx - vdir[0] * Dz; + if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false; + f = vdir[0] * Dy - vdir[1] * Dx; + if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false; + return true; + } + + + SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extend = m_max-center; + + btScalar _fOrigin = direction.dot(center); + btScalar _fMaximumExtent = extend.dot(direction.absolute()); + vmin = _fOrigin - _fMaximumExtent; + vmax = _fOrigin + _fMaximumExtent; + } + + SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const + { + btScalar _fmin,_fmax; + this->projection_interval(plane,_fmin,_fmax); + + if(plane[3] > _fmax + BOX_PLANE_EPSILON) + { + return G_BACK_PLANE; // 0 + } + + if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + { + return G_COLLIDE_PLANE; //1 + } + return G_FRONT_PLANE;//2 + } + + SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0) + { + GIM_AABB tbox = box; + tbox.appy_transform(trans1_to_0); + return has_collision(tbox); + } + + //! transcache is the transformation cache from box to this AABB + SIMD_FORCE_INLINE bool overlapping_trans_cache( + const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) + { + + //Taken from OPCODE + btVector3 ea,eb;//extends + btVector3 ca,cb;//extends + get_center_extend(ca,ea); + box.get_center_extend(cb,eb); + + + btVector3 T; + btScalar t,t2; + int i; + + // Class I : A's basis vectors + for(i=0;i<3;i++) + { + T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; + t = transcache.m_AR[i].dot(eb) + ea[i]; + if(GIM_GREATER(T[i], t)) return false; + } + // Class II : B's basis vectors + for(i=0;i<3;i++) + { + t = MAT_DOT_COL(transcache.m_R1to0,T,i); + t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i]; + if(GIM_GREATER(t,t2)) return false; + } + // Class III : 9 cross products + if(fulltest) + { + int j,m,n,o,p,q,r; + for(i=0;i<3;i++) + { + m = (i+1)%3; + n = (i+2)%3; + o = i==0?1:0; + p = i==2?1:2; + for(j=0;j<3;j++) + { + q = j==2?1:2; + r = j==0?1:0; + t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j]; + t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] + + eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r]; + if(GIM_GREATER(t,t2)) return false; + } + } + } + return true; + } + + //! Simple test for planes. + SIMD_FORCE_INLINE bool collide_plane( + const btVector4 & plane) + { + ePLANE_INTERSECTION_TYPE classify = plane_classify(plane); + return (classify == G_COLLIDE_PLANE); + } + + //! test for a triangle, with edges + SIMD_FORCE_INLINE bool collide_triangle_exact( + const btVector3 & p1, + const btVector3 & p2, + const btVector3 & p3, + const btVector4 & triangle_plane) + { + if(!collide_plane(triangle_plane)) return false; + + btVector3 center,extends; + this->get_center_extend(center,extends); + + const btVector3 v1(p1 - center); + const btVector3 v2(p2 - center); + const btVector3 v3(p3 - center); + + //First axis + btVector3 diff(v2 - v1); + btVector3 abs_diff = diff.absolute(); + //Test With X axis + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends); + //Test With Y axis + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends); + //Test With Z axis + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends); + + + diff = v3 - v2; + abs_diff = diff.absolute(); + //Test With X axis + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends); + //Test With Y axis + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends); + //Test With Z axis + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends); + + diff = v1 - v3; + abs_diff = diff.absolute(); + //Test With X axis + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends); + //Test With Y axis + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends); + //Test With Z axis + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends); + + return true; + } +}; + + +//! Compairison of transformation objects +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) +{ + if(!(t1.getOrigin() == t2.getOrigin()) ) return false; + + if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false; + if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false; + if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false; + return true; +} + + +//! @} + +#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp new file mode 100644 index 00000000000..1ffc2bbad78 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp @@ -0,0 +1,182 @@ + +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + + +#include "gim_box_set.h" + + +GUINT GIM_BOX_TREE::_calc_splitting_axis( + gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex) +{ + GUINT i; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); + GUINT numIndices = endIndex-startIndex; + + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means+=center; + } + means *= (btScalar(1.)/(btScalar)numIndices); + + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + btVector3 diff2 = center-means; + diff2 = diff2 * diff2; + variance += diff2; + } + variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + + return variance.maxAxis(); +} + + +GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index( + gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, + GUINT endIndex, GUINT splitAxis) +{ + GUINT i; + GUINT splitIndex =startIndex; + GUINT numIndices = endIndex - startIndex; + + // average of centers + btScalar splitValue = 0.0f; + for (i=startIndex;i<endIndex;i++) + { + splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] + + primitive_boxes[i].m_bound.m_min[splitAxis]); + } + splitValue /= (btScalar)numIndices; + + //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. + for (i=startIndex;i<endIndex;i++) + { + btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] + + primitive_boxes[i].m_bound.m_min[splitAxis]); + if (center > splitValue) + { + //swap + primitive_boxes.swap(i,splitIndex); + splitIndex++; + } + } + + //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: + GUINT 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; +} + + +void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex) +{ + GUINT current_index = m_num_nodes++; + + btAssert((endIndex-startIndex)>0); + + if((endIndex-startIndex) == 1) //we got a leaf + { + m_node_array[current_index].m_left = 0; + m_node_array[current_index].m_right = 0; + m_node_array[current_index].m_escapeIndex = 0; + + m_node_array[current_index].m_bound = primitive_boxes[startIndex].m_bound; + m_node_array[current_index].m_data = primitive_boxes[startIndex].m_data; + return; + } + + //configure inner node + + GUINT splitIndex; + + //calc this node bounding box + m_node_array[current_index].m_bound.invalidate(); + for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++) + { + m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound); + } + + //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + + //split axis + splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + + splitIndex = _sort_and_calc_splitting_index( + primitive_boxes,startIndex,endIndex,splitIndex); + + //configure this inner node : the left node index + m_node_array[current_index].m_left = m_num_nodes; + //build left child tree + _build_sub_tree(primitive_boxes, startIndex, splitIndex ); + + //configure this inner node : the right node index + m_node_array[current_index].m_right = m_num_nodes; + + //build right child tree + _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + + //configure this inner node : the escape index + m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index; +} + +//! stackless build tree +void GIM_BOX_TREE::build_tree( + gim_array<GIM_AABB_DATA> & primitive_boxes) +{ + // initialize node count to 0 + m_num_nodes = 0; + // allocate nodes + m_node_array.resize(primitive_boxes.size()*2); + + _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); +} + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h new file mode 100644 index 00000000000..ece5936de6b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h @@ -0,0 +1,678 @@ +#ifndef GIM_BOX_SET_H_INCLUDED +#define GIM_BOX_SET_H_INCLUDED + +/*! \file gim_box_set.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + + +#include "gim_array.h" +#include "gim_radixsort.h" +#include "gim_box_collision.h" +#include "gim_tri_collision.h" + + +/*! \defgroup BOX_PRUNNING + + + +*/ +//! @{ + +//! Overlapping pair +struct GIM_PAIR +{ + GUINT m_index1; + GUINT m_index2; + GIM_PAIR() + {} + + GIM_PAIR(const GIM_PAIR & p) + { + m_index1 = p.m_index1; + m_index2 = p.m_index2; + } + + GIM_PAIR(GUINT index1, GUINT index2) + { + m_index1 = index1; + m_index2 = index2; + } +}; + +//! A pairset array +class gim_pair_set: public gim_array<GIM_PAIR> +{ +public: + gim_pair_set():gim_array<GIM_PAIR>(32) + { + } + inline void push_pair(GUINT index1,GUINT index2) + { + push_back(GIM_PAIR(index1,index2)); + } + + inline void push_pair_inv(GUINT index1,GUINT index2) + { + push_back(GIM_PAIR(index2,index1)); + } +}; + + +//! Prototype Base class for primitive classification +/*! +This class is a wrapper for primitive collections. +This tells relevant info for the Bounding Box set classes, which take care of space classification. +This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons. +*/ +class GIM_PRIMITIVE_MANAGER_PROTOTYPE +{ +public: + + //! determines if this manager consist on only triangles, which special case will be optimized + virtual bool is_trimesh() = 0; + virtual GUINT get_primitive_count() = 0; + virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0; + virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0; +}; + + +struct GIM_AABB_DATA +{ + GIM_AABB m_bound; + GUINT m_data; +}; + +//! Node Structure for trees +struct GIM_BOX_TREE_NODE +{ + GIM_AABB m_bound; + GUINT m_left;//!< Left subtree + GUINT m_right;//!< Right subtree + GUINT m_escapeIndex;//!< Scape index for traversing + GUINT m_data;//!< primitive index if apply + + GIM_BOX_TREE_NODE() + { + m_left = 0; + m_right = 0; + m_escapeIndex = 0; + m_data = 0; + } + + SIMD_FORCE_INLINE bool is_leaf_node() const + { + return (!m_left && !m_right); + } +}; + +//! Basic Box tree structure +class GIM_BOX_TREE +{ +protected: + GUINT m_num_nodes; + gim_array<GIM_BOX_TREE_NODE> m_node_array; +protected: + GUINT _sort_and_calc_splitting_index( + gim_array<GIM_AABB_DATA> & primitive_boxes, + GUINT startIndex, GUINT endIndex, GUINT splitAxis); + + GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex); + + void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex); +public: + GIM_BOX_TREE() + { + m_num_nodes = 0; + } + + //! prototype functions for box tree management + //!@{ + void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes); + + SIMD_FORCE_INLINE void clearNodes() + { + m_node_array.clear(); + m_num_nodes = 0; + } + + //! node count + SIMD_FORCE_INLINE GUINT getNodeCount() const + { + return m_num_nodes; + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const + { + return m_node_array[nodeindex].is_leaf_node(); + } + + SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const + { + return m_node_array[nodeindex].m_data; + } + + SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + { + bound = m_node_array[nodeindex].m_bound; + } + + SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound) + { + m_node_array[nodeindex].m_bound = bound; + } + + SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const + { + return m_node_array[nodeindex].m_left; + } + + SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const + { + return m_node_array[nodeindex].m_right; + } + + SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const + { + return m_node_array[nodeindex].m_escapeIndex; + } + + //!@} +}; + + +//! Generic Box Tree Template +/*! +This class offers an structure for managing a box tree of primitives. +Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and +a Box tree structure ( like GIM_BOX_TREE). +*/ +template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE> +class GIM_BOX_TREE_TEMPLATE_SET +{ +protected: + _GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager; + _GIM_BOX_TREE_PROTOTYPE m_box_tree; +protected: + //stackless refit + SIMD_FORCE_INLINE void refit() + { + GUINT nodecount = getNodeCount(); + while(nodecount--) + { + if(isLeafNode(nodecount)) + { + GIM_AABB leafbox; + m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox); + setNodeBound(nodecount,leafbox); + } + else + { + //get left bound + GUINT childindex = getLeftNodeIndex(nodecount); + GIM_AABB bound; + getNodeBound(childindex,bound); + //get right bound + childindex = getRightNodeIndex(nodecount); + GIM_AABB bound2; + getNodeBound(childindex,bound2); + bound.merge(bound2); + + setNodeBound(nodecount,bound); + } + } + } +public: + + GIM_BOX_TREE_TEMPLATE_SET() + { + } + + SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const + { + GIM_AABB totalbox; + getNodeBound(0, totalbox); + return totalbox; + } + + SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const + { + return m_primitive_manager; + } + + _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() + { + return m_primitive_manager; + } + +//! node manager prototype functions +///@{ + + //! this attemps to refit the box set. + SIMD_FORCE_INLINE void update() + { + refit(); + } + + //! this rebuild the entire set + SIMD_FORCE_INLINE void buildSet() + { + //obtain primitive boxes + gim_array<GIM_AABB_DATA> primitive_boxes; + primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false); + + for (GUINT i = 0;i<primitive_boxes.size() ;i++ ) + { + m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; + } + + m_box_tree.build_tree(primitive_boxes); + } + + //! returns the indices of the primitives in the m_primitive_manager + SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const + { + GUINT curIndex = 0; + GUINT numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + GIM_AABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.has_collision(box); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData(curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getScapeNodeIndex(curIndex); + } + } + if(collided_results.size()>0) return true; + return false; + } + + //! returns the indices of the primitives in the m_primitive_manager + SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box, + const btTransform & transform, gim_array<GUINT> & collided_results) const + { + GIM_AABB transbox=box; + transbox.appy_transform(transform); + return boxQuery(transbox,collided_results); + } + + //! returns the indices of the primitives in the m_primitive_manager + SIMD_FORCE_INLINE bool rayQuery( + const btVector3 & ray_dir,const btVector3 & ray_origin , + gim_array<GUINT> & collided_results) const + { + GUINT curIndex = 0; + GUINT numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + GIM_AABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData( curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getScapeNodeIndex(curIndex); + } + } + if(collided_results.size()>0) return true; + return false; + } + + //! tells if this set has hierarcht + SIMD_FORCE_INLINE bool hasHierarchy() const + { + return true; + } + + //! tells if this set is a trimesh + SIMD_FORCE_INLINE bool isTrimesh() const + { + return m_primitive_manager.is_trimesh(); + } + + //! node count + SIMD_FORCE_INLINE GUINT getNodeCount() const + { + return m_box_tree.getNodeCount(); + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const + { + return m_box_tree.isLeafNode(nodeindex); + } + + SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const + { + return m_box_tree.getNodeData(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + { + m_box_tree.getNodeBound(nodeindex, bound); + } + + SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound) + { + m_box_tree.setNodeBound(nodeindex, bound); + } + + SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const + { + return m_box_tree.getLeftNodeIndex(nodeindex); + } + + SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const + { + return m_box_tree.getRightNodeIndex(nodeindex); + } + + SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const + { + return m_box_tree.getScapeNodeIndex(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const + { + m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle); + } + +//! @} +}; + +//! Class for Box Tree Sets +/*! +this has the GIM_BOX_TREE implementation for bounding boxes. +*/ +template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE> +class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE> +{ +public: + +}; + + + + + +/// GIM_BOX_SET collision methods +template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1> +class GIM_TREE_TREE_COLLIDER +{ +public: + gim_pair_set * m_collision_pairs; + BOX_SET_CLASS0 * m_boxset0; + BOX_SET_CLASS1 * m_boxset1; + GUINT current_node0; + GUINT current_node1; + bool node0_is_leaf; + bool node1_is_leaf; + bool t0_is_trimesh; + bool t1_is_trimesh; + bool node0_has_triangle; + bool node1_has_triangle; + GIM_AABB m_box0; + GIM_AABB m_box1; + GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; + btTransform trans_cache_0to1; + GIM_TRIANGLE m_tri0; + btVector4 m_tri0_plane; + GIM_TRIANGLE m_tri1; + btVector4 m_tri1_plane; + + +public: + GIM_TREE_TREE_COLLIDER() + { + current_node0 = G_UINT_INFINITY; + current_node1 = G_UINT_INFINITY; + } +protected: + SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0) + { + if(node0_has_triangle) return; + m_boxset0->getNodeTriangle(node0,m_tri0); + //transform triangle + m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]); + m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]); + m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]); + m_tri0.get_plane(m_tri0_plane); + + node0_has_triangle = true; + } + + SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1) + { + if(node1_has_triangle) return; + m_boxset1->getNodeTriangle(node1,m_tri1); + //transform triangle + m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]); + m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]); + m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]); + m_tri1.get_plane(m_tri1_plane); + + node1_has_triangle = true; + } + + SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0) + { + if(node0 == current_node0) return; + m_boxset0->getNodeBound(node0,m_box0); + node0_is_leaf = m_boxset0->isLeafNode(node0); + node0_has_triangle = false; + current_node0 = node0; + } + + SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1) + { + if(node1 == current_node1) return; + m_boxset1->getNodeBound(node1,m_box1); + node1_is_leaf = m_boxset1->isLeafNode(node1); + node1_has_triangle = false; + current_node1 = node1; + } + + SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1) + { + retrieve_node0_info(node0); + retrieve_node1_info(node1); + bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true); + if(!result) return false; + + if(t0_is_trimesh && node0_is_leaf) + { + //perform primitive vs box collision + retrieve_node0_triangle(node0); + //do triangle vs box collision + m_box1.increment_margin(m_tri0.m_margin); + + result = m_box1.collide_triangle_exact( + m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane); + + m_box1.increment_margin(-m_tri0.m_margin); + + if(!result) return false; + return true; + } + else if(t1_is_trimesh && node1_is_leaf) + { + //perform primitive vs box collision + retrieve_node1_triangle(node1); + //do triangle vs box collision + m_box0.increment_margin(m_tri1.m_margin); + + result = m_box0.collide_triangle_exact( + m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane); + + m_box0.increment_margin(-m_tri1.m_margin); + + if(!result) return false; + return true; + } + return true; + } + + //stackless collision routine + void find_collision_pairs() + { + gim_pair_set stack_collisions; + stack_collisions.reserve(32); + + //add the first pair + stack_collisions.push_pair(0,0); + + + while(stack_collisions.size()) + { + //retrieve the last pair and pop + GUINT node0 = stack_collisions.back().m_index1; + GUINT node1 = stack_collisions.back().m_index2; + stack_collisions.pop_back(); + if(node_collision(node0,node1)) // a collision is found + { + if(node0_is_leaf) + { + if(node1_is_leaf) + { + m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1)); + } + else + { + //collide left + stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1)); + + //collide right + stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1)); + } + } + else + { + if(node1_is_leaf) + { + //collide left + stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1); + //collide right + stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1); + } + else + { + GUINT left0 = m_boxset0->getLeftNodeIndex(node0); + GUINT right0 = m_boxset0->getRightNodeIndex(node0); + GUINT left1 = m_boxset1->getLeftNodeIndex(node1); + GUINT right1 = m_boxset1->getRightNodeIndex(node1); + //collide left + stack_collisions.push_pair(left0,left1); + //collide right + stack_collisions.push_pair(left0,right1); + //collide left + stack_collisions.push_pair(right0,left1); + //collide right + stack_collisions.push_pair(right0,right1); + + }// else if node1 is not a leaf + }// else if node0 is not a leaf + + }// if(node_collision(node0,node1)) + }//while(stack_collisions.size()) + } +public: + void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1, + BOX_SET_CLASS1 * boxset2, const btTransform & trans2, + gim_pair_set & collision_pairs, bool complete_primitive_tests = true) + { + m_collision_pairs = &collision_pairs; + m_boxset0 = boxset1; + m_boxset1 = boxset2; + + trans_cache_1to0.calc_from_homogenic(trans1,trans2); + + trans_cache_0to1 = trans2.inverse(); + trans_cache_0to1 *= trans1; + + + if(complete_primitive_tests) + { + t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh(); + t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh(); + } + else + { + t0_is_trimesh = false; + t1_is_trimesh = false; + } + + find_collision_pairs(); + } +}; + + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h new file mode 100644 index 00000000000..62151cb97b7 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h @@ -0,0 +1,214 @@ +#ifndef GIM_CLIP_POLYGON_H_INCLUDED +#define GIM_CLIP_POLYGON_H_INCLUDED + +/*! \file gim_tri_collision.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +/*! \addtogroup GEOMETRIC_OPERATIONS +*/ +//! @{ + +//! This function calcs the distance from a 3D plane +class DISTANCE_PLANE_3D_FUNC +{ +public: + template<typename CLASS_POINT,typename CLASS_PLANE> + inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point) + { + return DISTANCE_PLANE_POINT(plane, point); + } +}; + + + +template<typename CLASS_POINT> +SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT( + const CLASS_POINT & point0, + const CLASS_POINT & point1, + GREAL dist0, + GREAL dist1, + CLASS_POINT * clipped, + GUINT & clipped_count) +{ + GUINT _prevclassif = (dist0>G_EPSILON); + GUINT _classif = (dist1>G_EPSILON); + if(_classif!=_prevclassif) + { + GREAL blendfactor = -dist0/(dist1-dist0); + VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor); + clipped_count++; + } + if(!_classif) + { + VEC_COPY(clipped[clipped_count],point1); + clipped_count++; + } +} + + +//! Clips a polygon by a plane +/*! +*\return The count of the clipped counts +*/ +template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC> +SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC( + const CLASS_PLANE & plane, + const CLASS_POINT * polygon_points, + GUINT polygon_point_count, + CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func) +{ + GUINT clipped_count = 0; + + + //clip first point + GREAL firstdist = distance_func(plane,polygon_points[0]);; + if(!(firstdist>G_EPSILON)) + { + VEC_COPY(clipped[clipped_count],polygon_points[0]); + clipped_count++; + } + + GREAL olddist = firstdist; + for(GUINT _i=1;_i<polygon_point_count;_i++) + { + GREAL dist = distance_func(plane,polygon_points[_i]); + + PLANE_CLIP_POLYGON_COLLECT( + polygon_points[_i-1],polygon_points[_i], + olddist, + dist, + clipped, + clipped_count); + + + olddist = dist; + } + + //RETURN TO FIRST point + + PLANE_CLIP_POLYGON_COLLECT( + polygon_points[polygon_point_count-1],polygon_points[0], + olddist, + firstdist, + clipped, + clipped_count); + + return clipped_count; +} + +//! Clips a polygon by a plane +/*! +*\return The count of the clipped counts +*/ +template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC> +SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC( + const CLASS_PLANE & plane, + const CLASS_POINT & point0, + const CLASS_POINT & point1, + const CLASS_POINT & point2, + CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func) +{ + GUINT clipped_count = 0; + + //clip first point + GREAL firstdist = distance_func(plane,point0);; + if(!(firstdist>G_EPSILON)) + { + VEC_COPY(clipped[clipped_count],point0); + clipped_count++; + } + + // point 1 + GREAL olddist = firstdist; + GREAL dist = distance_func(plane,point1); + + PLANE_CLIP_POLYGON_COLLECT( + point0,point1, + olddist, + dist, + clipped, + clipped_count); + + olddist = dist; + + + // point 2 + dist = distance_func(plane,point2); + + PLANE_CLIP_POLYGON_COLLECT( + point1,point2, + olddist, + dist, + clipped, + clipped_count); + olddist = dist; + + + + //RETURN TO FIRST point + PLANE_CLIP_POLYGON_COLLECT( + point2,point0, + olddist, + firstdist, + clipped, + clipped_count); + + return clipped_count; +} + + +template<typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D( + const CLASS_PLANE & plane, + const CLASS_POINT * polygon_points, + GUINT polygon_point_count, + CLASS_POINT * clipped) +{ + return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC()); +} + + +template<typename CLASS_POINT,typename CLASS_PLANE> +SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D( + const CLASS_PLANE & plane, + const CLASS_POINT & point0, + const CLASS_POINT & point1, + const CLASS_POINT & point2, + CLASS_POINT * clipped) +{ + return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC()); +} + + +//! @} + +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp new file mode 100644 index 00000000000..20e41de089f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp @@ -0,0 +1,146 @@ + +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "gim_contact.h" + +#define MAX_COINCIDENT 8 + +void gim_contact_array::merge_contacts( + const gim_contact_array & contacts, bool normal_contact_average) +{ + clear(); + + if(contacts.size()==1) + { + push_back(contacts.back()); + return; + } + + gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size()); + keycontacts.resize(contacts.size(),false); + + //fill key contacts + + GUINT i; + + for (i = 0;i<contacts.size() ;i++ ) + { + keycontacts[i].m_key = contacts[i].calc_key_contact(); + keycontacts[i].m_value = i; + } + + //sort keys + gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR()); + + // Merge contacts + + GUINT coincident_count=0; + btVector3 coincident_normals[MAX_COINCIDENT]; + + GUINT last_key = keycontacts[0].m_key; + GUINT key = 0; + + push_back(contacts[keycontacts[0].m_value]); + GIM_CONTACT * pcontact = &back(); + + + + for( i=1;i<keycontacts.size();i++) + { + key = keycontacts[i].m_key; + const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; + + if(last_key == key)//same points + { + //merge contact + if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + { + *pcontact = *scontact; + coincident_count = 0; + } + else if(normal_contact_average) + { + if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON) + { + if(coincident_count<MAX_COINCIDENT) + { + coincident_normals[coincident_count] = scontact->m_normal; + coincident_count++; + } + } + } + } + else + {//add new contact + + if(normal_contact_average && coincident_count>0) + { + pcontact->interpolate_normals(coincident_normals,coincident_count); + coincident_count = 0; + } + + push_back(*scontact); + pcontact = &back(); + } + last_key = key; + } +} + +void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts) +{ + clear(); + + if(contacts.size()==1) + { + push_back(contacts.back()); + return; + } + + GIM_CONTACT average_contact = contacts.back(); + + for (GUINT i=1;i<contacts.size() ;i++ ) + { + average_contact.m_point += contacts[i].m_point; + average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth; + } + + //divide + GREAL divide_average = 1.0f/((GREAL)contacts.size()); + + average_contact.m_point *= divide_average; + + average_contact.m_normal *= divide_average; + + average_contact.m_depth = average_contact.m_normal.length(); + + average_contact.m_normal /= average_contact.m_depth; + +} + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h new file mode 100644 index 00000000000..62df51b782f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h @@ -0,0 +1,172 @@ +#ifndef GIM_CONTACT_H_INCLUDED +#define GIM_CONTACT_H_INCLUDED + +/*! \file gim_contact.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ +#include "gim_geometry.h" +#include "gim_radixsort.h" +#include "gim_array.h" + +/*! \defgroup CONTACTS +\brief +Functions for managing and sorting contacts resulting from a collision query. +<ul> +<li> Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST +<li> After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY +<li> Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts +</ul> + +*/ +//! @{ + +/** +Configuration var for applying interpolation of contact normals +*/ +#define NORMAL_CONTACT_AVERAGE 1 +#define CONTACT_DIFF_EPSILON 0.00001f + +/// Structure for collision results +class GIM_CONTACT +{ +public: + btVector3 m_point; + btVector3 m_normal; + GREAL m_depth;//Positive value indicates interpenetration + GREAL m_distance;//Padding not for use + GUINT m_feature1;//Face number + GUINT m_feature2;//Face number +public: + GIM_CONTACT() + { + } + + GIM_CONTACT(const GIM_CONTACT & contact): + m_point(contact.m_point), + m_normal(contact.m_normal), + m_depth(contact.m_depth), + m_feature1(contact.m_feature1), + m_feature2(contact.m_feature2) + { + m_point = contact.m_point; + m_normal = contact.m_normal; + m_depth = contact.m_depth; + m_feature1 = contact.m_feature1; + m_feature2 = contact.m_feature2; + } + + GIM_CONTACT(const btVector3 &point,const btVector3 & normal, + GREAL depth, GUINT feature1, GUINT feature2): + m_point(point), + m_normal(normal), + m_depth(depth), + m_feature1(feature1), + m_feature2(feature2) + { + } + + //! Calcs key for coord classification + SIMD_FORCE_INLINE GUINT calc_key_contact() const + { + GINT _coords[] = { + (GINT)(m_point[0]*1000.0f+1.0f), + (GINT)(m_point[1]*1333.0f), + (GINT)(m_point[2]*2133.0f+3.0f)}; + GUINT _hash=0; + GUINT *_uitmp = (GUINT *)(&_coords[0]); + _hash = *_uitmp; + _uitmp++; + _hash += (*_uitmp)<<4; + _uitmp++; + _hash += (*_uitmp)<<8; + return _hash; + } + + SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count) + { + btVector3 vec_sum(m_normal); + for(GUINT i=0;i<normal_count;i++) + { + vec_sum += normals[i]; + } + + GREAL vec_sum_len = vec_sum.length2(); + if(vec_sum_len <CONTACT_DIFF_EPSILON) return; + + GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len) + + m_normal = vec_sum*vec_sum_len; + } + +}; + + +class gim_contact_array:public gim_array<GIM_CONTACT> +{ +public: + gim_contact_array():gim_array<GIM_CONTACT>(64) + { + } + + SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal, + GREAL depth, GUINT feature1, GUINT feature2) + { + push_back_mem(); + GIM_CONTACT & newele = back(); + newele.m_point = point; + newele.m_normal = normal; + newele.m_depth = depth; + newele.m_feature1 = feature1; + newele.m_feature2 = feature2; + } + + SIMD_FORCE_INLINE void push_triangle_contacts( + const GIM_TRIANGLE_CONTACT_DATA & tricontact, + GUINT feature1,GUINT feature2) + { + for(GUINT i = 0;i<tricontact.m_point_count ;i++ ) + { + push_back_mem(); + GIM_CONTACT & newele = back(); + newele.m_point = tricontact.m_points[i]; + newele.m_normal = tricontact.m_separating_normal; + newele.m_depth = tricontact.m_penetration_depth; + newele.m_feature1 = feature1; + newele.m_feature2 = feature2; + } + } + + void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true); + void merge_contacts_unique(const gim_contact_array & contacts); +}; + +//! @} +#endif // GIM_CONTACT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h new file mode 100644 index 00000000000..cd3e53dcc0c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h @@ -0,0 +1,102 @@ +#ifndef GIM_GEOM_TYPES_H_INCLUDED +#define GIM_GEOM_TYPES_H_INCLUDED + +/*! \file gim_geom_types.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + + +#include "gim_math.h" + +/*! \defgroup GEOMETRIC_TYPES +\brief +Basic types and constants for geometry +*/ +//! @{ + +//! Short Integer vector 2D +typedef GSHORT vec2s[2]; +//! Integer vector 3D +typedef GSHORT vec3s[3]; +//! Integer vector 4D +typedef GSHORT vec4s[4]; + +//! Short Integer vector 2D +typedef GUSHORT vec2us[2]; +//! Integer vector 3D +typedef GUSHORT vec3us[3]; +//! Integer vector 4D +typedef GUSHORT vec4us[4]; + +//! Integer vector 2D +typedef GINT vec2i[2]; +//! Integer vector 3D +typedef GINT vec3i[3]; +//! Integer vector 4D +typedef GINT vec4i[4]; + +//! Unsigned Integer vector 2D +typedef GUINT vec2ui[2]; +//! Unsigned Integer vector 3D +typedef GUINT vec3ui[3]; +//! Unsigned Integer vector 4D +typedef GUINT vec4ui[4]; + +//! Float vector 2D +typedef GREAL vec2f[2]; +//! Float vector 3D +typedef GREAL vec3f[3]; +//! Float vector 4D +typedef GREAL vec4f[4]; + +//! Double vector 2D +typedef GREAL2 vec2d[2]; +//! Float vector 3D +typedef GREAL2 vec3d[3]; +//! Float vector 4D +typedef GREAL2 vec4d[4]; + +//! Matrix 2D, row ordered +typedef GREAL mat2f[2][2]; +//! Matrix 3D, row ordered +typedef GREAL mat3f[3][3]; +//! Matrix 4D, row ordered +typedef GREAL mat4f[4][4]; + +//! Quaternion +typedef GREAL quatf[4]; + +//typedef struct _aabb3f aabb3f; +//! @} + + + +#endif // GIM_GEOM_TYPES_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h new file mode 100644 index 00000000000..6cc41672101 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h @@ -0,0 +1,42 @@ +#ifndef GIM_GEOMETRY_H_INCLUDED +#define GIM_GEOMETRY_H_INCLUDED + +/*! \file gim_geometry.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +///Additional Headers for Collision +#include "gim_basic_geometry_operations.h" +#include "gim_clip_polygon.h" +#include "gim_box_collision.h" +#include "gim_tri_collision.h" + +#endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h new file mode 100644 index 00000000000..fd980dff5fa --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h @@ -0,0 +1,920 @@ +#ifndef GIM_HASH_TABLE_H_INCLUDED +#define GIM_HASH_TABLE_H_INCLUDED +/*! \file gim_trimesh_data.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "gim_radixsort.h" + +/*! \addtogroup CONTAINERS +\brief +Abstract class for collision objects +*/ +//! @{ + +#define GIM_INVALID_HASH 0xffffffff //!< A very very high value +#define GIM_DEFAULT_HASH_TABLE_SIZE 380 +#define GIM_DEFAULT_HASH_TABLE_NODE_SIZE 4 +#define GIM_HASH_TABLE_GROW_FACTOR 2 + +#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII + +template<typename T> +struct GIM_HASH_TABLE_NODE +{ + GUINT m_key; + T m_data; + GIM_HASH_TABLE_NODE() + { + } + + GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value) + { + m_key = value.m_key; + m_data = value.m_data; + } + + GIM_HASH_TABLE_NODE(GUINT key, const T & data) + { + m_key = key; + m_data = data; + } + + bool operator <(const GIM_HASH_TABLE_NODE<T> & other) const + { + ///inverse order, further objects are first + if(m_key < other.m_key) return true; + return false; + } + + bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const + { + ///inverse order, further objects are first + if(m_key > other.m_key) return true; + return false; + } + + bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const + { + ///inverse order, further objects are first + if(m_key == other.m_key) return true; + return false; + } +}; + +///Macro for getting the key +class GIM_HASH_NODE_GET_KEY +{ +public: + template<class T> + inline GUINT operator()( const T& a) + { + return a.m_key; + } +}; + + + +///Macro for comparing the key and the element +class GIM_HASH_NODE_CMP_KEY_MACRO +{ +public: + template<class T> + inline int operator() ( const T& a, GUINT key) + { + return ((int)(a.m_key - key)); + } +}; + +///Macro for comparing Hash nodes +class GIM_HASH_NODE_CMP_MACRO +{ +public: + template<class T> + inline int operator() ( const T& a, const T& b ) + { + return ((int)(a.m_key - b.m_key)); + } +}; + + + + + +//! Sorting for hash table +/*! +switch automatically between quicksort and radixsort +*/ +template<typename T> +void gim_sort_hash_node_array(T * array, GUINT array_count) +{ + if(array_count<GIM_MIN_RADIX_SORT_SIZE) + { + gim_heap_sort(array,array_count,GIM_HASH_NODE_CMP_MACRO()); + } + else + { + memcopy_elements_func cmpfunc; + gim_radix_sort(array,array_count,GIM_HASH_NODE_GET_KEY(),cmpfunc); + } +} + + + + + + +// Note: assumes long is at least 32 bits. +#define GIM_NUM_PRIME 28 + +static const GUINT gim_prime_list[GIM_NUM_PRIME] = +{ + 53ul, 97ul, 193ul, 389ul, 769ul, + 1543ul, 3079ul, 6151ul, 12289ul, 24593ul, + 49157ul, 98317ul, 196613ul, 393241ul, 786433ul, + 1572869ul, 3145739ul, 6291469ul, 12582917ul, 25165843ul, + 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul, + 1610612741ul, 3221225473ul, 4294967291ul +}; + +inline GUINT gim_next_prime(GUINT number) +{ + //Find nearest upper prime + GUINT result_ind = 0; + gim_binary_search(gim_prime_list,0,(GIM_NUM_PRIME-2),number,result_ind); + + // inv: result_ind < 28 + return gim_prime_list[result_ind]; +} + + + +//! A compact hash table implementation +/*! +A memory aligned compact hash table that coud be treated as an array. +It could be a simple sorted array without the overhead of the hash key bucked, or could +be a formely hash table with an array of keys. +You can use switch_to_hashtable() and switch_to_sorted_array for saving space or increase speed. +</br> + +<ul> +<li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes. +When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable. +<li> If node_size != 0, then this container becomes a hash table for ever +</ul> + +*/ +template<class T> +class gim_hash_table +{ +protected: + typedef GIM_HASH_TABLE_NODE<T> _node_type; + + //!The nodes + //array< _node_type, SuperAllocator<_node_type> > m_nodes; + gim_array< _node_type > m_nodes; + //SuperBufferedArray< _node_type > m_nodes; + bool m_sorted; + + /*! \defgroup HASH_TABLE_STRUCTURES + \brief + Hash table data management. The hash table has the indices to the corresponding m_nodes array + */ + //! @{ + + GUINT * m_hash_table;//!< + GUINT m_table_size;//!< + GUINT m_node_size;//!< + GUINT m_min_hash_table_size; + + + + //! Returns the cell index + inline GUINT _find_cell(GUINT hashkey) + { + _node_type * nodesptr = m_nodes.pointer(); + GUINT start_index = (hashkey%m_table_size)*m_node_size; + GUINT end_index = start_index + m_node_size; + + while(start_index<end_index) + { + GUINT value = m_hash_table[start_index]; + if(value != GIM_INVALID_HASH) + { + if(nodesptr[value].m_key == hashkey) return start_index; + } + start_index++; + } + return GIM_INVALID_HASH; + } + + //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key + inline GUINT _find_avaliable_cell(GUINT hashkey) + { + _node_type * nodesptr = m_nodes.pointer(); + GUINT avaliable_index = GIM_INVALID_HASH; + GUINT start_index = (hashkey%m_table_size)*m_node_size; + GUINT end_index = start_index + m_node_size; + + while(start_index<end_index) + { + GUINT value = m_hash_table[start_index]; + if(value == GIM_INVALID_HASH) + { + if(avaliable_index==GIM_INVALID_HASH) + { + avaliable_index = start_index; + } + } + else if(nodesptr[value].m_key == hashkey) + { + return start_index; + } + start_index++; + } + return avaliable_index; + } + + + + //! reserves the memory for the hash table. + /*! + \pre hash table must be empty + \post reserves the memory for the hash table, an initializes all elements to GIM_INVALID_HASH. + */ + inline void _reserve_table_memory(GUINT newtablesize) + { + if(newtablesize==0) return; + if(m_node_size==0) return; + + //Get a Prime size + + m_table_size = gim_next_prime(newtablesize); + + GUINT datasize = m_table_size*m_node_size; + //Alloc the data buffer + m_hash_table = (GUINT *)gim_alloc(datasize*sizeof(GUINT)); + } + + inline void _invalidate_keys() + { + GUINT datasize = m_table_size*m_node_size; + for(GUINT i=0;i<datasize;i++) + { + m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys + } + } + + //! Clear all memory for the hash table + inline void _clear_table_memory() + { + if(m_hash_table==NULL) return; + gim_free(m_hash_table); + m_hash_table = NULL; + m_table_size = 0; + } + + //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys + inline void _rehash() + { + _invalidate_keys(); + + _node_type * nodesptr = m_nodes.pointer(); + for(GUINT i=0;i<(GUINT)m_nodes.size();i++) + { + GUINT nodekey = nodesptr[i].m_key; + if(nodekey != GIM_INVALID_HASH) + { + //Search for the avaliable cell in buffer + GUINT index = _find_avaliable_cell(nodekey); + + + if(m_hash_table[index]!=GIM_INVALID_HASH) + {//The new index is alreade used... discard this new incomming object, repeated key + btAssert(m_hash_table[index]==nodekey); + nodesptr[i].m_key = GIM_INVALID_HASH; + } + else + { + //; + //Assign the value for alloc + m_hash_table[index] = i; + } + } + } + } + + //! Resize hash table indices + inline void _resize_table(GUINT newsize) + { + //Clear memory + _clear_table_memory(); + //Alloc the data + _reserve_table_memory(newsize); + //Invalidate keys and rehash + _rehash(); + } + + //! Destroy hash table memory + inline void _destroy() + { + if(m_hash_table==NULL) return; + _clear_table_memory(); + } + + //! Finds an avaliable hash table cell, and resizes the table if there isn't space + inline GUINT _assign_hash_table_cell(GUINT hashkey) + { + GUINT cell_index = _find_avaliable_cell(hashkey); + + if(cell_index==GIM_INVALID_HASH) + { + //rehashing + _resize_table(m_table_size+1); + GUINT cell_index = _find_avaliable_cell(hashkey); + btAssert(cell_index!=GIM_INVALID_HASH); + } + return cell_index; + } + + //! erase by index in hash table + inline bool _erase_by_index_hash_table(GUINT index) + { + if(index >= m_nodes.size()) return false; + if(m_nodes[index].m_key != GIM_INVALID_HASH) + { + //Search for the avaliable cell in buffer + GUINT cell_index = _find_cell(m_nodes[index].m_key); + + btAssert(cell_index!=GIM_INVALID_HASH); + btAssert(m_hash_table[cell_index]==index); + + m_hash_table[cell_index] = GIM_INVALID_HASH; + } + + return this->_erase_unsorted(index); + } + + //! erase by key in hash table + inline bool _erase_hash_table(GUINT hashkey) + { + if(hashkey == GIM_INVALID_HASH) return false; + + //Search for the avaliable cell in buffer + GUINT cell_index = _find_cell(hashkey); + if(cell_index ==GIM_INVALID_HASH) return false; + + GUINT index = m_hash_table[cell_index]; + m_hash_table[cell_index] = GIM_INVALID_HASH; + + return this->_erase_unsorted(index); + } + + + + //! insert an element in hash table + /*! + If the element exists, this won't insert the element + \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted + If so, the element has been inserted at the last position of the array. + */ + inline GUINT _insert_hash_table(GUINT hashkey, const T & value) + { + if(hashkey==GIM_INVALID_HASH) + { + //Insert anyway + _insert_unsorted(hashkey,value); + return GIM_INVALID_HASH; + } + + GUINT cell_index = _assign_hash_table_cell(hashkey); + + GUINT value_key = m_hash_table[cell_index]; + + if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited + + m_hash_table[cell_index] = m_nodes.size(); + + _insert_unsorted(hashkey,value); + return GIM_INVALID_HASH; + } + + //! insert an element in hash table. + /*! + If the element exists, this replaces the element. + \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted + If so, the element has been inserted at the last position of the array. + */ + inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value) + { + if(hashkey==GIM_INVALID_HASH) + { + //Insert anyway + _insert_unsorted(hashkey,value); + return GIM_INVALID_HASH; + } + + GUINT cell_index = _assign_hash_table_cell(hashkey); + + GUINT value_key = m_hash_table[cell_index]; + + if(value_key!= GIM_INVALID_HASH) + {//replaces the existing + m_nodes[value_key] = _node_type(hashkey,value); + return value_key;// index of the replaced element + } + + m_hash_table[cell_index] = m_nodes.size(); + + _insert_unsorted(hashkey,value); + return GIM_INVALID_HASH; + + } + + //! @} + + /*! \defgroup SORTED_ARRAY_STRUCTURES + \brief + Sorted array data management. The hash table has the indices to the corresponding m_nodes array + */ + //! @{ + inline bool _erase_sorted(GUINT index) + { + if(index>=(GUINT)m_nodes.size()) return false; + m_nodes.erase_sorted(index); + if(m_nodes.size()<2) m_sorted = false; + return true; + } + + //! faster, but unsorted + inline bool _erase_unsorted(GUINT index) + { + if(index>=m_nodes.size()) return false; + + GUINT lastindex = m_nodes.size()-1; + if(index<lastindex && m_hash_table!=0) + { + GUINT hashkey = m_nodes[lastindex].m_key; + if(hashkey!=GIM_INVALID_HASH) + { + //update the new position of the last element + GUINT cell_index = _find_cell(hashkey); + btAssert(cell_index!=GIM_INVALID_HASH); + //new position of the last element which will be swaped + m_hash_table[cell_index] = index; + } + } + m_nodes.erase(index); + m_sorted = false; + return true; + } + + //! Insert in position ordered + /*! + Also checks if it is needed to transform this container to a hash table, by calling check_for_switching_to_hashtable + */ + inline void _insert_in_pos(GUINT hashkey, const T & value, GUINT pos) + { + m_nodes.insert(_node_type(hashkey,value),pos); + this->check_for_switching_to_hashtable(); + } + + //! Insert an element in an ordered array + inline GUINT _insert_sorted(GUINT hashkey, const T & value) + { + if(hashkey==GIM_INVALID_HASH || size()==0) + { + m_nodes.push_back(_node_type(hashkey,value)); + return GIM_INVALID_HASH; + } + //Insert at last position + //Sort element + + + GUINT result_ind=0; + GUINT last_index = m_nodes.size()-1; + _node_type * ptr = m_nodes.pointer(); + + bool found = gim_binary_search_ex( + ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); + + + //Insert before found index + if(found) + { + return result_ind; + } + else + { + _insert_in_pos(hashkey, value, result_ind); + } + return GIM_INVALID_HASH; + } + + inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value) + { + if(hashkey==GIM_INVALID_HASH || size()==0) + { + m_nodes.push_back(_node_type(hashkey,value)); + return GIM_INVALID_HASH; + } + //Insert at last position + //Sort element + GUINT result_ind; + GUINT last_index = m_nodes.size()-1; + _node_type * ptr = m_nodes.pointer(); + + bool found = gim_binary_search_ex( + ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); + + //Insert before found index + if(found) + { + m_nodes[result_ind] = _node_type(hashkey,value); + } + else + { + _insert_in_pos(hashkey, value, result_ind); + } + return result_ind; + } + + //! Fast insertion in m_nodes array + inline GUINT _insert_unsorted(GUINT hashkey, const T & value) + { + m_nodes.push_back(_node_type(hashkey,value)); + m_sorted = false; + return GIM_INVALID_HASH; + } + + //! @} + + +public: + + /*! + <li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes. + When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable. + <li> If node_size != 0, then this container becomes a hash table for ever + </ul> + */ + gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE, + GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE, + GUINT min_hash_table_size = GIM_INVALID_HASH) + { + m_hash_table = NULL; + m_table_size = 0; + m_sorted = false; + m_node_size = node_size; + m_min_hash_table_size = min_hash_table_size; + + if(m_node_size!=0) + { + if(reserve_size!=0) + { + m_nodes.reserve(reserve_size); + _reserve_table_memory(reserve_size); + _invalidate_keys(); + } + else + { + m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE); + _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE); + _invalidate_keys(); + } + } + else if(reserve_size!=0) + { + m_nodes.reserve(reserve_size); + } + + } + + ~gim_hash_table() + { + _destroy(); + } + + inline bool is_hash_table() + { + if(m_hash_table) return true; + return false; + } + + inline bool is_sorted() + { + if(size()<2) return true; + return m_sorted; + } + + bool sort() + { + if(is_sorted()) return true; + if(m_nodes.size()<2) return false; + + + _node_type * ptr = m_nodes.pointer(); + GUINT siz = m_nodes.size(); + gim_sort_hash_node_array(ptr,siz); + m_sorted=true; + + + + if(m_hash_table) + { + _rehash(); + } + return true; + } + + bool switch_to_hashtable() + { + if(m_hash_table) return false; + if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; + if(m_nodes.size()<GIM_DEFAULT_HASH_TABLE_SIZE) + { + _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE); + } + else + { + _resize_table(m_nodes.size()+1); + } + + return true; + } + + bool switch_to_sorted_array() + { + if(m_hash_table==NULL) return true; + _clear_table_memory(); + return sort(); + } + + //!If the container reaches the + bool check_for_switching_to_hashtable() + { + if(this->m_hash_table) return true; + + if(!(m_nodes.size()< m_min_hash_table_size)) + { + if(m_node_size == 0) + { + m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; + } + + _resize_table(m_nodes.size()+1); + return true; + } + return false; + } + + inline void set_sorted(bool value) + { + m_sorted = value; + } + + //! Retrieves the amount of keys. + inline GUINT size() const + { + return m_nodes.size(); + } + + //! Retrieves the hash key. + inline GUINT get_key(GUINT index) const + { + return m_nodes[index].m_key; + } + + //! Retrieves the value by index + /*! + */ + inline T * get_value_by_index(GUINT index) + { + return &m_nodes[index].m_data; + } + + inline const T& operator[](GUINT index) const + { + return m_nodes[index].m_data; + } + + inline T& operator[](GUINT index) + { + return m_nodes[index].m_data; + } + + //! Finds the index of the element with the key + /*! + \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted + If so, the element has been inserted at the last position of the array. + */ + inline GUINT find(GUINT hashkey) + { + if(m_hash_table) + { + GUINT cell_index = _find_cell(hashkey); + if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH; + return m_hash_table[cell_index]; + } + GUINT last_index = m_nodes.size(); + if(last_index<2) + { + if(last_index==0) return GIM_INVALID_HASH; + if(m_nodes[0].m_key == hashkey) return 0; + return GIM_INVALID_HASH; + } + else if(m_sorted) + { + //Binary search + GUINT result_ind = 0; + last_index--; + _node_type * ptr = m_nodes.pointer(); + + bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); + + + if(found) return result_ind; + } + return GIM_INVALID_HASH; + } + + //! Retrieves the value associated with the index + /*! + \return the found element, or null + */ + inline T * get_value(GUINT hashkey) + { + GUINT index = find(hashkey); + if(index == GIM_INVALID_HASH) return NULL; + return &m_nodes[index].m_data; + } + + + /*! + */ + inline bool erase_by_index(GUINT index) + { + if(index > m_nodes.size()) return false; + + if(m_hash_table == NULL) + { + if(is_sorted()) + { + return this->_erase_sorted(index); + } + else + { + return this->_erase_unsorted(index); + } + } + else + { + return this->_erase_by_index_hash_table(index); + } + return false; + } + + + + inline bool erase_by_index_unsorted(GUINT index) + { + if(index > m_nodes.size()) return false; + + if(m_hash_table == NULL) + { + return this->_erase_unsorted(index); + } + else + { + return this->_erase_by_index_hash_table(index); + } + return false; + } + + + + /*! + + */ + inline bool erase_by_key(GUINT hashkey) + { + if(size()==0) return false; + + if(m_hash_table) + { + return this->_erase_hash_table(hashkey); + } + //Binary search + + if(is_sorted()==false) return false; + + GUINT result_ind = find(hashkey); + if(result_ind!= GIM_INVALID_HASH) + { + return this->_erase_sorted(result_ind); + } + return false; + } + + void clear() + { + m_nodes.clear(); + + if(m_hash_table==NULL) return; + GUINT datasize = m_table_size*m_node_size; + //Initialize the hashkeys. + GUINT i; + for(i=0;i<datasize;i++) + { + m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys + } + m_sorted = false; + } + + //! Insert an element into the hash + /*! + \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position + of the existing element. + */ + inline GUINT insert(GUINT hashkey, const T & element) + { + if(m_hash_table) + { + return this->_insert_hash_table(hashkey,element); + } + if(this->is_sorted()) + { + return this->_insert_sorted(hashkey,element); + } + return this->_insert_unsorted(hashkey,element); + } + + //! Insert an element into the hash, and could overrite an existing object with the same hash. + /*! + \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position + of the replaced element. + */ + inline GUINT insert_override(GUINT hashkey, const T & element) + { + if(m_hash_table) + { + return this->_insert_hash_table_replace(hashkey,element); + } + if(this->is_sorted()) + { + return this->_insert_sorted_replace(hashkey,element); + } + this->_insert_unsorted(hashkey,element); + return m_nodes.size(); + } + + + + //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted + /*! + */ + inline GUINT insert_unsorted(GUINT hashkey,const T & element) + { + if(m_hash_table) + { + return this->_insert_hash_table(hashkey,element); + } + return this->_insert_unsorted(hashkey,element); + } + + +}; + +//! @} + + + +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h new file mode 100644 index 00000000000..adf1c1c2ddf --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h @@ -0,0 +1,1584 @@ +#ifndef GIM_LINEAR_H_INCLUDED +#define GIM_LINEAR_H_INCLUDED + +/*! \file gim_linear_math.h +*\author Francisco Len Nßjera +Type Independant Vector and matrix operations. +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + + +#include "gim_math.h" +#include "gim_geom_types.h" + + + +/*! \defgroup VECTOR_OPERATIONS +T +Operations for vectors : vec2f,vec3f and vec4f +*/ +//! @{ + +//! Zero out a 2D vector +#define VEC_ZERO_2(a) \ +{ \ + (a)[0] = (a)[1] = 0.0f; \ +}\ + + +//! Zero out a 3D vector +#define VEC_ZERO(a) \ +{ \ + (a)[0] = (a)[1] = (a)[2] = 0.0f; \ +}\ + + +/// Zero out a 4D vector +#define VEC_ZERO_4(a) \ +{ \ + (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \ +}\ + + +/// Vector copy +#define VEC_COPY_2(b,a) \ +{ \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ +}\ + + +/// Copy 3D vector +#define VEC_COPY(b,a) \ +{ \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + (b)[2] = (a)[2]; \ +}\ + + +/// Copy 4D vector +#define VEC_COPY_4(b,a) \ +{ \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + (b)[2] = (a)[2]; \ + (b)[3] = (a)[3]; \ +}\ + +/// VECTOR SWAP +#define VEC_SWAP(b,a) \ +{ \ + GIM_SWAP_NUMBERS((b)[0],(a)[0]);\ + GIM_SWAP_NUMBERS((b)[1],(a)[1]);\ + GIM_SWAP_NUMBERS((b)[2],(a)[2]);\ +}\ + +/// Vector difference +#define VEC_DIFF_2(v21,v2,v1) \ +{ \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ +}\ + + +/// Vector difference +#define VEC_DIFF(v21,v2,v1) \ +{ \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + (v21)[2] = (v2)[2] - (v1)[2]; \ +}\ + + +/// Vector difference +#define VEC_DIFF_4(v21,v2,v1) \ +{ \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + (v21)[2] = (v2)[2] - (v1)[2]; \ + (v21)[3] = (v2)[3] - (v1)[3]; \ +}\ + + +/// Vector sum +#define VEC_SUM_2(v21,v2,v1) \ +{ \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ +}\ + + +/// Vector sum +#define VEC_SUM(v21,v2,v1) \ +{ \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + (v21)[2] = (v2)[2] + (v1)[2]; \ +}\ + + +/// Vector sum +#define VEC_SUM_4(v21,v2,v1) \ +{ \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + (v21)[2] = (v2)[2] + (v1)[2]; \ + (v21)[3] = (v2)[3] + (v1)[3]; \ +}\ + + +/// scalar times vector +#define VEC_SCALE_2(c,a,b) \ +{ \ + (c)[0] = (a)*(b)[0]; \ + (c)[1] = (a)*(b)[1]; \ +}\ + + +/// scalar times vector +#define VEC_SCALE(c,a,b) \ +{ \ + (c)[0] = (a)*(b)[0]; \ + (c)[1] = (a)*(b)[1]; \ + (c)[2] = (a)*(b)[2]; \ +}\ + + +/// scalar times vector +#define VEC_SCALE_4(c,a,b) \ +{ \ + (c)[0] = (a)*(b)[0]; \ + (c)[1] = (a)*(b)[1]; \ + (c)[2] = (a)*(b)[2]; \ + (c)[3] = (a)*(b)[3]; \ +}\ + + +/// accumulate scaled vector +#define VEC_ACCUM_2(c,a,b) \ +{ \ + (c)[0] += (a)*(b)[0]; \ + (c)[1] += (a)*(b)[1]; \ +}\ + + +/// accumulate scaled vector +#define VEC_ACCUM(c,a,b) \ +{ \ + (c)[0] += (a)*(b)[0]; \ + (c)[1] += (a)*(b)[1]; \ + (c)[2] += (a)*(b)[2]; \ +}\ + + +/// accumulate scaled vector +#define VEC_ACCUM_4(c,a,b) \ +{ \ + (c)[0] += (a)*(b)[0]; \ + (c)[1] += (a)*(b)[1]; \ + (c)[2] += (a)*(b)[2]; \ + (c)[3] += (a)*(b)[3]; \ +}\ + + +/// Vector dot product +#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1]) + + +/// Vector dot product +#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]) + +/// Vector dot product +#define VEC_DOT_4(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3]) + +/// vector impact parameter (squared) +#define VEC_IMPACT_SQ(bsq,direction,position) {\ + GREAL _llel_ = VEC_DOT(direction, position);\ + bsq = VEC_DOT(position, position) - _llel_*_llel_;\ +}\ + + +/// vector impact parameter +#define VEC_IMPACT(bsq,direction,position) {\ + VEC_IMPACT_SQ(bsq,direction,position); \ + GIM_SQRT(bsq,bsq); \ +}\ + +/// Vector length +#define VEC_LENGTH_2(a,l)\ +{\ + GREAL _pp = VEC_DOT_2(a,a);\ + GIM_SQRT(_pp,l);\ +}\ + + +/// Vector length +#define VEC_LENGTH(a,l)\ +{\ + GREAL _pp = VEC_DOT(a,a);\ + GIM_SQRT(_pp,l);\ +}\ + + +/// Vector length +#define VEC_LENGTH_4(a,l)\ +{\ + GREAL _pp = VEC_DOT_4(a,a);\ + GIM_SQRT(_pp,l);\ +}\ + +/// Vector inv length +#define VEC_INV_LENGTH_2(a,l)\ +{\ + GREAL _pp = VEC_DOT_2(a,a);\ + GIM_INV_SQRT(_pp,l);\ +}\ + + +/// Vector inv length +#define VEC_INV_LENGTH(a,l)\ +{\ + GREAL _pp = VEC_DOT(a,a);\ + GIM_INV_SQRT(_pp,l);\ +}\ + + +/// Vector inv length +#define VEC_INV_LENGTH_4(a,l)\ +{\ + GREAL _pp = VEC_DOT_4(a,a);\ + GIM_INV_SQRT(_pp,l);\ +}\ + + + +/// distance between two points +#define VEC_DISTANCE(_len,_va,_vb) {\ + vec3f _tmp_; \ + VEC_DIFF(_tmp_, _vb, _va); \ + VEC_LENGTH(_tmp_,_len); \ +}\ + + +/// Vector length +#define VEC_CONJUGATE_LENGTH(a,l)\ +{\ + GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\ + GIM_SQRT(_pp,l);\ +}\ + + +/// Vector length +#define VEC_NORMALIZE(a) { \ + GREAL len;\ + VEC_INV_LENGTH(a,len); \ + if(len<G_REAL_INFINITY)\ + {\ + a[0] *= len; \ + a[1] *= len; \ + a[2] *= len; \ + } \ +}\ + +/// Set Vector size +#define VEC_RENORMALIZE(a,newlen) { \ + GREAL len;\ + VEC_INV_LENGTH(a,len); \ + if(len<G_REAL_INFINITY)\ + {\ + len *= newlen;\ + a[0] *= len; \ + a[1] *= len; \ + a[2] *= len; \ + } \ +}\ + +/// Vector cross +#define VEC_CROSS(c,a,b) \ +{ \ + c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \ + c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \ + c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \ +}\ + + +/*! Vector perp -- assumes that n is of unit length + * accepts vector v, subtracts out any component parallel to n */ +#define VEC_PERPENDICULAR(vp,v,n) \ +{ \ + GREAL dot = VEC_DOT(v, n); \ + vp[0] = (v)[0] - dot*(n)[0]; \ + vp[1] = (v)[1] - dot*(n)[1]; \ + vp[2] = (v)[2] - dot*(n)[2]; \ +}\ + + +/*! Vector parallel -- assumes that n is of unit length */ +#define VEC_PARALLEL(vp,v,n) \ +{ \ + GREAL dot = VEC_DOT(v, n); \ + vp[0] = (dot) * (n)[0]; \ + vp[1] = (dot) * (n)[1]; \ + vp[2] = (dot) * (n)[2]; \ +}\ + +/*! Same as Vector parallel -- n can have any length + * accepts vector v, subtracts out any component perpendicular to n */ +#define VEC_PROJECT(vp,v,n) \ +{ \ + GREAL scalar = VEC_DOT(v, n); \ + scalar/= VEC_DOT(n, n); \ + vp[0] = (scalar) * (n)[0]; \ + vp[1] = (scalar) * (n)[1]; \ + vp[2] = (scalar) * (n)[2]; \ +}\ + + +/*! accepts vector v*/ +#define VEC_UNPROJECT(vp,v,n) \ +{ \ + GREAL scalar = VEC_DOT(v, n); \ + scalar = VEC_DOT(n, n)/scalar; \ + vp[0] = (scalar) * (n)[0]; \ + vp[1] = (scalar) * (n)[1]; \ + vp[2] = (scalar) * (n)[2]; \ +}\ + + +/*! Vector reflection -- assumes n is of unit length + Takes vector v, reflects it against reflector n, and returns vr */ +#define VEC_REFLECT(vr,v,n) \ +{ \ + GREAL dot = VEC_DOT(v, n); \ + vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \ + vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \ + vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \ +}\ + + +/*! Vector blending +Takes two vectors a, b, blends them together with two scalars */ +#define VEC_BLEND_AB(vr,sa,a,sb,b) \ +{ \ + vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \ + vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \ + vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \ +}\ + +/*! Vector blending +Takes two vectors a, b, blends them together with s <=1 */ +#define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b) + +#define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; + +//! Finds the bigger cartesian coordinate from a vector +#define VEC_MAYOR_COORD(vec, maxc)\ +{\ + GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\ + maxc = A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\ +}\ + +//! Finds the 2 smallest cartesian coordinates from a vector +#define VEC_MINOR_AXES(vec, i0, i1)\ +{\ + VEC_MAYOR_COORD(vec,i0);\ + i0 = (i0+1)%3;\ + i1 = (i0+1)%3;\ +}\ + + + + +#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2]) + +#define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2])) + + +/// Vector cross +#define X_AXIS_CROSS_VEC(dst,src)\ +{ \ + dst[0] = 0.0f; \ + dst[1] = -src[2]; \ + dst[2] = src[1]; \ +}\ + +#define Y_AXIS_CROSS_VEC(dst,src)\ +{ \ + dst[0] = src[2]; \ + dst[1] = 0.0f; \ + dst[2] = -src[0]; \ +}\ + +#define Z_AXIS_CROSS_VEC(dst,src)\ +{ \ + dst[0] = -src[1]; \ + dst[1] = src[0]; \ + dst[2] = 0.0f; \ +}\ + + + +//! @} + + +/*! \defgroup MATRIX_OPERATIONS +Operations for matrices : mat2f, mat3f and mat4f +*/ +//! @{ + +/// initialize matrix +#define IDENTIFY_MATRIX_3X3(m) \ +{ \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ +}\ + +/*! initialize matrix */ +#define IDENTIFY_MATRIX_4X4(m) \ +{ \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ +}\ + +/*! initialize matrix */ +#define ZERO_MATRIX_4X4(m) \ +{ \ + m[0][0] = 0.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 0.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 0.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 0.0; \ +}\ + +/*! matrix rotation X */ +#define ROTX_CS(m,cosine,sine) \ +{ \ + /* rotation about the x-axis */ \ + \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = (cosine); \ + m[1][2] = (sine); \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = -(sine); \ + m[2][2] = (cosine); \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ +}\ + +/*! matrix rotation Y */ +#define ROTY_CS(m,cosine,sine) \ +{ \ + /* rotation about the y-axis */ \ + \ + m[0][0] = (cosine); \ + m[0][1] = 0.0; \ + m[0][2] = -(sine); \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = (sine); \ + m[2][1] = 0.0; \ + m[2][2] = (cosine); \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ +}\ + +/*! matrix rotation Z */ +#define ROTZ_CS(m,cosine,sine) \ +{ \ + /* rotation about the z-axis */ \ + \ + m[0][0] = (cosine); \ + m[0][1] = (sine); \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = -(sine); \ + m[1][1] = (cosine); \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ +}\ + +/*! matrix copy */ +#define COPY_MATRIX_2X2(b,a) \ +{ \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + \ +}\ + + +/*! matrix copy */ +#define COPY_MATRIX_2X3(b,a) \ +{ \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ +}\ + + +/*! matrix copy */ +#define COPY_MATRIX_3X3(b,a) \ +{ \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + \ + b[2][0] = a[2][0]; \ + b[2][1] = a[2][1]; \ + b[2][2] = a[2][2]; \ +}\ + + +/*! matrix copy */ +#define COPY_MATRIX_4X4(b,a) \ +{ \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + b[0][3] = a[0][3]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + b[1][3] = a[1][3]; \ + \ + b[2][0] = a[2][0]; \ + b[2][1] = a[2][1]; \ + b[2][2] = a[2][2]; \ + b[2][3] = a[2][3]; \ + \ + b[3][0] = a[3][0]; \ + b[3][1] = a[3][1]; \ + b[3][2] = a[3][2]; \ + b[3][3] = a[3][3]; \ +}\ + + +/*! matrix transpose */ +#define TRANSPOSE_MATRIX_2X2(b,a) \ +{ \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ +}\ + + +/*! matrix transpose */ +#define TRANSPOSE_MATRIX_3X3(b,a) \ +{ \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + b[0][2] = a[2][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[2][1]; \ + \ + b[2][0] = a[0][2]; \ + b[2][1] = a[1][2]; \ + b[2][2] = a[2][2]; \ +}\ + + +/*! matrix transpose */ +#define TRANSPOSE_MATRIX_4X4(b,a) \ +{ \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + b[0][2] = a[2][0]; \ + b[0][3] = a[3][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[2][1]; \ + b[1][3] = a[3][1]; \ + \ + b[2][0] = a[0][2]; \ + b[2][1] = a[1][2]; \ + b[2][2] = a[2][2]; \ + b[2][3] = a[3][2]; \ + \ + b[3][0] = a[0][3]; \ + b[3][1] = a[1][3]; \ + b[3][2] = a[2][3]; \ + b[3][3] = a[3][3]; \ +}\ + + +/*! multiply matrix by scalar */ +#define SCALE_MATRIX_2X2(b,s,a) \ +{ \ + b[0][0] = (s) * a[0][0]; \ + b[0][1] = (s) * a[0][1]; \ + \ + b[1][0] = (s) * a[1][0]; \ + b[1][1] = (s) * a[1][1]; \ +}\ + + +/*! multiply matrix by scalar */ +#define SCALE_MATRIX_3X3(b,s,a) \ +{ \ + b[0][0] = (s) * a[0][0]; \ + b[0][1] = (s) * a[0][1]; \ + b[0][2] = (s) * a[0][2]; \ + \ + b[1][0] = (s) * a[1][0]; \ + b[1][1] = (s) * a[1][1]; \ + b[1][2] = (s) * a[1][2]; \ + \ + b[2][0] = (s) * a[2][0]; \ + b[2][1] = (s) * a[2][1]; \ + b[2][2] = (s) * a[2][2]; \ +}\ + + +/*! multiply matrix by scalar */ +#define SCALE_MATRIX_4X4(b,s,a) \ +{ \ + b[0][0] = (s) * a[0][0]; \ + b[0][1] = (s) * a[0][1]; \ + b[0][2] = (s) * a[0][2]; \ + b[0][3] = (s) * a[0][3]; \ + \ + b[1][0] = (s) * a[1][0]; \ + b[1][1] = (s) * a[1][1]; \ + b[1][2] = (s) * a[1][2]; \ + b[1][3] = (s) * a[1][3]; \ + \ + b[2][0] = (s) * a[2][0]; \ + b[2][1] = (s) * a[2][1]; \ + b[2][2] = (s) * a[2][2]; \ + b[2][3] = (s) * a[2][3]; \ + \ + b[3][0] = s * a[3][0]; \ + b[3][1] = s * a[3][1]; \ + b[3][2] = s * a[3][2]; \ + b[3][3] = s * a[3][3]; \ +}\ + + +/*! multiply matrix by scalar */ +#define SCALE_VEC_MATRIX_2X2(b,svec,a) \ +{ \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ +}\ + + +/*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */ +#define SCALE_VEC_MATRIX_3X3(b,svec,a) \ +{ \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + b[2][0] = svec[0] * a[2][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + b[2][1] = svec[1] * a[2][1]; \ + \ + b[0][2] = svec[2] * a[0][2]; \ + b[1][2] = svec[2] * a[1][2]; \ + b[2][2] = svec[2] * a[2][2]; \ +}\ + + +/*! multiply matrix by scalar */ +#define SCALE_VEC_MATRIX_4X4(b,svec,a) \ +{ \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + b[2][0] = svec[0] * a[2][0]; \ + b[3][0] = svec[0] * a[3][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + b[2][1] = svec[1] * a[2][1]; \ + b[3][1] = svec[1] * a[3][1]; \ + \ + b[0][2] = svec[2] * a[0][2]; \ + b[1][2] = svec[2] * a[1][2]; \ + b[2][2] = svec[2] * a[2][2]; \ + b[3][2] = svec[2] * a[3][2]; \ + \ + b[0][3] = svec[3] * a[0][3]; \ + b[1][3] = svec[3] * a[1][3]; \ + b[2][3] = svec[3] * a[2][3]; \ + b[3][3] = svec[3] * a[3][3]; \ +}\ + + +/*! multiply matrix by scalar */ +#define ACCUM_SCALE_MATRIX_2X2(b,s,a) \ +{ \ + b[0][0] += (s) * a[0][0]; \ + b[0][1] += (s) * a[0][1]; \ + \ + b[1][0] += (s) * a[1][0]; \ + b[1][1] += (s) * a[1][1]; \ +}\ + + +/*! multiply matrix by scalar */ +#define ACCUM_SCALE_MATRIX_3X3(b,s,a) \ +{ \ + b[0][0] += (s) * a[0][0]; \ + b[0][1] += (s) * a[0][1]; \ + b[0][2] += (s) * a[0][2]; \ + \ + b[1][0] += (s) * a[1][0]; \ + b[1][1] += (s) * a[1][1]; \ + b[1][2] += (s) * a[1][2]; \ + \ + b[2][0] += (s) * a[2][0]; \ + b[2][1] += (s) * a[2][1]; \ + b[2][2] += (s) * a[2][2]; \ +}\ + + +/*! multiply matrix by scalar */ +#define ACCUM_SCALE_MATRIX_4X4(b,s,a) \ +{ \ + b[0][0] += (s) * a[0][0]; \ + b[0][1] += (s) * a[0][1]; \ + b[0][2] += (s) * a[0][2]; \ + b[0][3] += (s) * a[0][3]; \ + \ + b[1][0] += (s) * a[1][0]; \ + b[1][1] += (s) * a[1][1]; \ + b[1][2] += (s) * a[1][2]; \ + b[1][3] += (s) * a[1][3]; \ + \ + b[2][0] += (s) * a[2][0]; \ + b[2][1] += (s) * a[2][1]; \ + b[2][2] += (s) * a[2][2]; \ + b[2][3] += (s) * a[2][3]; \ + \ + b[3][0] += (s) * a[3][0]; \ + b[3][1] += (s) * a[3][1]; \ + b[3][2] += (s) * a[3][2]; \ + b[3][3] += (s) * a[3][3]; \ +}\ + +/*! matrix product */ +/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ +#define MATRIX_PRODUCT_2X2(c,a,b) \ +{ \ + c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]; \ + c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]; \ + \ + c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]; \ + c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]; \ + \ +}\ + +/*! matrix product */ +/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ +#define MATRIX_PRODUCT_3X3(c,a,b) \ +{ \ + c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; \ + c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; \ + c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; \ + \ + c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; \ + c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; \ + c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; \ + \ + c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; \ + c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; \ + c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; \ +}\ + + +/*! matrix product */ +/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ +#define MATRIX_PRODUCT_4X4(c,a,b) \ +{ \ + c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\ + c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\ + c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\ + c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\ + \ + c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\ + c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\ + c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\ + c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\ + \ + c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\ + c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\ + c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\ + c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\ + \ + c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\ + c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\ + c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\ + c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\ +}\ + + +/*! matrix times vector */ +#define MAT_DOT_VEC_2X2(p,m,v) \ +{ \ + p[0] = m[0][0]*v[0] + m[0][1]*v[1]; \ + p[1] = m[1][0]*v[0] + m[1][1]*v[1]; \ +}\ + + +/*! matrix times vector */ +#define MAT_DOT_VEC_3X3(p,m,v) \ +{ \ + p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; \ + p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; \ + p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; \ +}\ + + +/*! matrix times vector +v is a vec4f +*/ +#define MAT_DOT_VEC_4X4(p,m,v) \ +{ \ + p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3]; \ + p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3]; \ + p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3]; \ + p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3]; \ +}\ + +/*! matrix times vector +v is a vec3f +and m is a mat4f<br> +Last column is added as the position +*/ +#define MAT_DOT_VEC_3X4(p,m,v) \ +{ \ + p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \ + p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \ + p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \ +}\ + + +/*! vector transpose times matrix */ +/*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */ +#define VEC_DOT_MAT_3X3(p,v,m) \ +{ \ + p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]; \ + p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]; \ + p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]; \ +}\ + + +/*! affine matrix times vector */ +/** The matrix is assumed to be an affine matrix, with last two + * entries representing a translation */ +#define MAT_DOT_VEC_2X3(p,m,v) \ +{ \ + p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]; \ + p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \ +}\ + +//! Transform a plane +#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\ +{ \ + pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1] + m[0][2]*plane[2];\ + pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1] + m[1][2]*plane[2];\ + pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1] + m[2][2]*plane[2];\ + pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1] + m[2][3]*pout[2] + plane[3];\ +}\ + + + +/** inverse transpose of matrix times vector + * + * This macro computes inverse transpose of matrix m, + * and multiplies vector v into it, to yeild vector p + * + * DANGER !!! Do Not use this on normal vectors!!! + * It will leave normals the wrong length !!! + * See macro below for use on normals. + */ +#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \ +{ \ + GREAL det; \ + \ + det = m[0][0]*m[1][1] - m[0][1]*m[1][0]; \ + p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ + p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ + \ + /* if matrix not singular, and not orthonormal, then renormalize */ \ + if ((det!=1.0f) && (det != 0.0f)) { \ + det = 1.0f / det; \ + p[0] *= det; \ + p[1] *= det; \ + } \ +}\ + + +/** transform normal vector by inverse transpose of matrix + * and then renormalize the vector + * + * This macro computes inverse transpose of matrix m, + * and multiplies vector v into it, to yeild vector p + * Vector p is then normalized. + */ +#define NORM_XFORM_2X2(p,m,v) \ +{ \ + GREAL len; \ + \ + /* do nothing if off-diagonals are zero and diagonals are \ + * equal */ \ + if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \ + p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ + p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ + \ + len = p[0]*p[0] + p[1]*p[1]; \ + GIM_INV_SQRT(len,len); \ + p[0] *= len; \ + p[1] *= len; \ + } else { \ + VEC_COPY_2 (p, v); \ + } \ +}\ + + +/** outer product of vector times vector transpose + * + * The outer product of vector v and vector transpose t yeilds + * dyadic matrix m. + */ +#define OUTER_PRODUCT_2X2(m,v,t) \ +{ \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ +}\ + + +/** outer product of vector times vector transpose + * + * The outer product of vector v and vector transpose t yeilds + * dyadic matrix m. + */ +#define OUTER_PRODUCT_3X3(m,v,t) \ +{ \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + m[0][2] = v[0] * t[2]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + m[1][2] = v[1] * t[2]; \ + \ + m[2][0] = v[2] * t[0]; \ + m[2][1] = v[2] * t[1]; \ + m[2][2] = v[2] * t[2]; \ +}\ + + +/** outer product of vector times vector transpose + * + * The outer product of vector v and vector transpose t yeilds + * dyadic matrix m. + */ +#define OUTER_PRODUCT_4X4(m,v,t) \ +{ \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + m[0][2] = v[0] * t[2]; \ + m[0][3] = v[0] * t[3]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + m[1][2] = v[1] * t[2]; \ + m[1][3] = v[1] * t[3]; \ + \ + m[2][0] = v[2] * t[0]; \ + m[2][1] = v[2] * t[1]; \ + m[2][2] = v[2] * t[2]; \ + m[2][3] = v[2] * t[3]; \ + \ + m[3][0] = v[3] * t[0]; \ + m[3][1] = v[3] * t[1]; \ + m[3][2] = v[3] * t[2]; \ + m[3][3] = v[3] * t[3]; \ +}\ + + +/** outer product of vector times vector transpose + * + * The outer product of vector v and vector transpose t yeilds + * dyadic matrix m. + */ +#define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \ +{ \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ +}\ + + +/** outer product of vector times vector transpose + * + * The outer product of vector v and vector transpose t yeilds + * dyadic matrix m. + */ +#define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \ +{ \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + m[0][2] += v[0] * t[2]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + m[1][2] += v[1] * t[2]; \ + \ + m[2][0] += v[2] * t[0]; \ + m[2][1] += v[2] * t[1]; \ + m[2][2] += v[2] * t[2]; \ +}\ + + +/** outer product of vector times vector transpose + * + * The outer product of vector v and vector transpose t yeilds + * dyadic matrix m. + */ +#define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \ +{ \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + m[0][2] += v[0] * t[2]; \ + m[0][3] += v[0] * t[3]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + m[1][2] += v[1] * t[2]; \ + m[1][3] += v[1] * t[3]; \ + \ + m[2][0] += v[2] * t[0]; \ + m[2][1] += v[2] * t[1]; \ + m[2][2] += v[2] * t[2]; \ + m[2][3] += v[2] * t[3]; \ + \ + m[3][0] += v[3] * t[0]; \ + m[3][1] += v[3] * t[1]; \ + m[3][2] += v[3] * t[2]; \ + m[3][3] += v[3] * t[3]; \ +}\ + + +/** determinant of matrix + * + * Computes determinant of matrix m, returning d + */ +#define DETERMINANT_2X2(d,m) \ +{ \ + d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ +}\ + + +/** determinant of matrix + * + * Computes determinant of matrix m, returning d + */ +#define DETERMINANT_3X3(d,m) \ +{ \ + d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); \ + d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); \ + d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); \ +}\ + + +/** i,j,th cofactor of a 4x4 matrix + * + */ +#define COFACTOR_4X4_IJ(fac,m,i,j) \ +{ \ + GUINT __ii[4], __jj[4], __k; \ + \ + for (__k=0; __k<i; __k++) __ii[__k] = __k; \ + for (__k=i; __k<3; __k++) __ii[__k] = __k+1; \ + for (__k=0; __k<j; __k++) __jj[__k] = __k; \ + for (__k=j; __k<3; __k++) __jj[__k] = __k+1; \ + \ + (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] \ + - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \ + (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]] \ + - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\ + (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]] \ + - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\ + \ + __k = i+j; \ + if ( __k != (__k/2)*2) { \ + (fac) = -(fac); \ + } \ +}\ + + +/** determinant of matrix + * + * Computes determinant of matrix m, returning d + */ +#define DETERMINANT_4X4(d,m) \ +{ \ + GREAL cofac; \ + COFACTOR_4X4_IJ (cofac, m, 0, 0); \ + d = m[0][0] * cofac; \ + COFACTOR_4X4_IJ (cofac, m, 0, 1); \ + d += m[0][1] * cofac; \ + COFACTOR_4X4_IJ (cofac, m, 0, 2); \ + d += m[0][2] * cofac; \ + COFACTOR_4X4_IJ (cofac, m, 0, 3); \ + d += m[0][3] * cofac; \ +}\ + + +/** cofactor of matrix + * + * Computes cofactor of matrix m, returning a + */ +#define COFACTOR_2X2(a,m) \ +{ \ + a[0][0] = (m)[1][1]; \ + a[0][1] = - (m)[1][0]; \ + a[1][0] = - (m)[0][1]; \ + a[1][1] = (m)[0][0]; \ +}\ + + +/** cofactor of matrix + * + * Computes cofactor of matrix m, returning a + */ +#define COFACTOR_3X3(a,m) \ +{ \ + a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ + a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ + a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ + a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ + a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ + a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ + a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ + a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ + a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ +}\ + + +/** cofactor of matrix + * + * Computes cofactor of matrix m, returning a + */ +#define COFACTOR_4X4(a,m) \ +{ \ + int i,j; \ + \ + for (i=0; i<4; i++) { \ + for (j=0; j<4; j++) { \ + COFACTOR_4X4_IJ (a[i][j], m, i, j); \ + } \ + } \ +}\ + + +/** adjoint of matrix + * + * Computes adjoint of matrix m, returning a + * (Note that adjoint is just the transpose of the cofactor matrix) + */ +#define ADJOINT_2X2(a,m) \ +{ \ + a[0][0] = (m)[1][1]; \ + a[1][0] = - (m)[1][0]; \ + a[0][1] = - (m)[0][1]; \ + a[1][1] = (m)[0][0]; \ +}\ + + +/** adjoint of matrix + * + * Computes adjoint of matrix m, returning a + * (Note that adjoint is just the transpose of the cofactor matrix) + */ +#define ADJOINT_3X3(a,m) \ +{ \ + a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ + a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ + a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ + a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ + a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ + a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ + a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ + a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ + a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ +}\ + + +/** adjoint of matrix + * + * Computes adjoint of matrix m, returning a + * (Note that adjoint is just the transpose of the cofactor matrix) + */ +#define ADJOINT_4X4(a,m) \ +{ \ + char _i_,_j_; \ + \ + for (_i_=0; _i_<4; _i_++) { \ + for (_j_=0; _j_<4; _j_++) { \ + COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ + } \ + } \ +}\ + + +/** compute adjoint of matrix and scale + * + * Computes adjoint of matrix m, scales it by s, returning a + */ +#define SCALE_ADJOINT_2X2(a,s,m) \ +{ \ + a[0][0] = (s) * m[1][1]; \ + a[1][0] = - (s) * m[1][0]; \ + a[0][1] = - (s) * m[0][1]; \ + a[1][1] = (s) * m[0][0]; \ +}\ + + +/** compute adjoint of matrix and scale + * + * Computes adjoint of matrix m, scales it by s, returning a + */ +#define SCALE_ADJOINT_3X3(a,s,m) \ +{ \ + a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ + a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \ + a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ + \ + a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \ + a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \ + a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \ + \ + a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \ + a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \ + a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \ +}\ + + +/** compute adjoint of matrix and scale + * + * Computes adjoint of matrix m, scales it by s, returning a + */ +#define SCALE_ADJOINT_4X4(a,s,m) \ +{ \ + char _i_,_j_; \ + for (_i_=0; _i_<4; _i_++) { \ + for (_j_=0; _j_<4; _j_++) { \ + COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ + a[_j_][_i_] *= s; \ + } \ + } \ +}\ + +/** inverse of matrix + * + * Compute inverse of matrix a, returning determinant m and + * inverse b + */ +#define INVERT_2X2(b,det,a) \ +{ \ + GREAL _tmp_; \ + DETERMINANT_2X2 (det, a); \ + _tmp_ = 1.0 / (det); \ + SCALE_ADJOINT_2X2 (b, _tmp_, a); \ +}\ + + +/** inverse of matrix + * + * Compute inverse of matrix a, returning determinant m and + * inverse b + */ +#define INVERT_3X3(b,det,a) \ +{ \ + GREAL _tmp_; \ + DETERMINANT_3X3 (det, a); \ + _tmp_ = 1.0 / (det); \ + SCALE_ADJOINT_3X3 (b, _tmp_, a); \ +}\ + + +/** inverse of matrix + * + * Compute inverse of matrix a, returning determinant m and + * inverse b + */ +#define INVERT_4X4(b,det,a) \ +{ \ + GREAL _tmp_; \ + DETERMINANT_4X4 (det, a); \ + _tmp_ = 1.0 / (det); \ + SCALE_ADJOINT_4X4 (b, _tmp_, a); \ +}\ + +//! Get the triple(3) row of a transform matrix +#define MAT_GET_ROW(mat,vec3,rowindex)\ +{\ + vec3[0] = mat[rowindex][0];\ + vec3[1] = mat[rowindex][1];\ + vec3[2] = mat[rowindex][2]; \ +}\ + +//! Get the tuple(2) row of a transform matrix +#define MAT_GET_ROW2(mat,vec2,rowindex)\ +{\ + vec2[0] = mat[rowindex][0];\ + vec2[1] = mat[rowindex][1];\ +}\ + + +//! Get the quad (4) row of a transform matrix +#define MAT_GET_ROW4(mat,vec4,rowindex)\ +{\ + vec4[0] = mat[rowindex][0];\ + vec4[1] = mat[rowindex][1];\ + vec4[2] = mat[rowindex][2];\ + vec4[3] = mat[rowindex][3];\ +}\ + +//! Get the triple(3) col of a transform matrix +#define MAT_GET_COL(mat,vec3,colindex)\ +{\ + vec3[0] = mat[0][colindex];\ + vec3[1] = mat[1][colindex];\ + vec3[2] = mat[2][colindex]; \ +}\ + +//! Get the tuple(2) col of a transform matrix +#define MAT_GET_COL2(mat,vec2,colindex)\ +{\ + vec2[0] = mat[0][colindex];\ + vec2[1] = mat[1][colindex];\ +}\ + + +//! Get the quad (4) col of a transform matrix +#define MAT_GET_COL4(mat,vec4,colindex)\ +{\ + vec4[0] = mat[0][colindex];\ + vec4[1] = mat[1][colindex];\ + vec4[2] = mat[2][colindex];\ + vec4[3] = mat[3][colindex];\ +}\ + +//! Get the triple(3) col of a transform matrix +#define MAT_GET_X(mat,vec3)\ +{\ + MAT_GET_COL(mat,vec3,0);\ +}\ + +//! Get the triple(3) col of a transform matrix +#define MAT_GET_Y(mat,vec3)\ +{\ + MAT_GET_COL(mat,vec3,1);\ +}\ + +//! Get the triple(3) col of a transform matrix +#define MAT_GET_Z(mat,vec3)\ +{\ + MAT_GET_COL(mat,vec3,2);\ +}\ + + +//! Get the triple(3) col of a transform matrix +#define MAT_SET_X(mat,vec3)\ +{\ + mat[0][0] = vec3[0];\ + mat[1][0] = vec3[1];\ + mat[2][0] = vec3[2];\ +}\ + +//! Get the triple(3) col of a transform matrix +#define MAT_SET_Y(mat,vec3)\ +{\ + mat[0][1] = vec3[0];\ + mat[1][1] = vec3[1];\ + mat[2][1] = vec3[2];\ +}\ + +//! Get the triple(3) col of a transform matrix +#define MAT_SET_Z(mat,vec3)\ +{\ + mat[0][2] = vec3[0];\ + mat[1][2] = vec3[1];\ + mat[2][2] = vec3[2];\ +}\ + + +//! Get the triple(3) col of a transform matrix +#define MAT_GET_TRANSLATION(mat,vec3)\ +{\ + vec3[0] = mat[0][3];\ + vec3[1] = mat[1][3];\ + vec3[2] = mat[2][3]; \ +}\ + +//! Set the triple(3) col of a transform matrix +#define MAT_SET_TRANSLATION(mat,vec3)\ +{\ + mat[0][3] = vec3[0];\ + mat[1][3] = vec3[1];\ + mat[2][3] = vec3[2]; \ +}\ + + + +//! Returns the dot product between a vec3f and the row of a matrix +#define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2]) + +//! Returns the dot product between a vec2f and the row of a matrix +#define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1]) + +//! Returns the dot product between a vec4f and the row of a matrix +#define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3]) + + +//! Returns the dot product between a vec3f and the col of a matrix +#define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex]) + +//! Returns the dot product between a vec2f and the col of a matrix +#define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex]) + +//! Returns the dot product between a vec4f and the col of a matrix +#define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex]) + +/*!Transpose matrix times vector +v is a vec3f +and m is a mat4f<br> +*/ +#define INV_MAT_DOT_VEC_3X3(p,m,v) \ +{ \ + p[0] = MAT_DOT_COL(m,v,0); \ + p[1] = MAT_DOT_COL(m,v,1); \ + p[2] = MAT_DOT_COL(m,v,2); \ +}\ + + +//! @} + +#endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h new file mode 100644 index 00000000000..25ad1e21c7f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h @@ -0,0 +1,181 @@ +#ifndef GIM_MATH_H_INCLUDED +#define GIM_MATH_H_INCLUDED +/*! \file gim_math.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "LinearMath/btScalar.h" + + +/*! \defgroup BASIC_TYPES +Basic types and constants +Conventions: +Types starting with G +Constants starting with G_ +*/ +//! @{ + +#define GREAL btScalar +#define GREAL2 double +#define GINT int +#define GUINT unsigned int +#define GSHORT short +#define GUSHORT unsigned short +#define GINT64 long long +#define GUINT64 unsigned long long + +//! @} + +/*! \defgroup BASIC_CONSTANTS +Basic constants +Conventions: +Types starting with G +Constants starting with G_ +*/ +//! @{ + +#define G_PI 3.14159265358979f +#define G_HALF_PI 1.5707963f +//267948966 +#define G_TWO_PI 6.28318530f +//71795864 +#define G_ROOT3 1.73205f +#define G_ROOT2 1.41421f +#define G_UINT_INFINITY 0xffffffff //!< A very very high value +#define G_REAL_INFINITY FLT_MAX +#define G_SIGN_BITMASK 0x80000000 +#define G_EPSILON SIMD_EPSILON +//! @} + + +/*! \defgroup SCALAR_TYPES +\brief +Precision type constants +*/ +//! @{ +enum GIM_SCALAR_TYPES +{ + G_STYPE_REAL =0, + G_STYPE_REAL2, + G_STYPE_SHORT, + G_STYPE_USHORT, + G_STYPE_INT, + G_STYPE_UINT, + G_STYPE_INT64, + G_STYPE_UINT64 +}; +//! @} + +/*! \defgroup MATH_FUNCTIONS +mathematical functions +*/ +//! @{ + +#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f) +#define G_RADTODEG(X) ((X)*180.0f/3.1415926f) + +//! Integer representation of a floating-point value. +#define GIM_IR(x) ((GUINT&)(x)) + +//! Signed integer representation of a floating-point value. +#define GIM_SIR(x) ((GINT&)(x)) + +//! Absolute integer representation of a floating-point value +#define GIM_AIR(x) (GIM_IR(x)&0x7fffffff) + +//! Floating-point representation of an integer value. +#define GIM_FR(x) ((GREAL&)(x)) + +#define GIM_MAX(a,b) (a<b?b:a) +#define GIM_MIN(a,b) (a>b?b:a) + +#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c)) +#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c)) + +#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON) + +#define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON) + +#define GIM_IS_POSISITVE(value) (value >= G_EPSILON) + +#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2)) + +///returns a clamped number +#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number)) + +#define GIM_GREATER(x, y) fabsf(x) > (y) + +///Swap numbers +#define GIM_SWAP_NUMBERS(a,b){ \ + a = a+b; \ + b = a-b; \ + a = a-b; \ +}\ + +#define GIM_INV_SQRT(va,isva)\ +{\ + if(va<=0.0000001f)\ + {\ + isva = G_REAL_INFINITY;\ + }\ + else\ + {\ + GREAL _x = va * 0.5f;\ + GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\ + isva = GIM_FR(_y);\ + isva = isva * ( 1.5f - ( _x * isva * isva ) );\ + }\ +}\ + +#define GIM_SQRT(va,sva)\ +{\ + GIM_INV_SQRT(va,sva);\ + sva = 1.0f/sva;\ +}\ + +//! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html +inline GREAL gim_inv_sqrt(GREAL f) +{ + GREAL r; + GIM_INV_SQRT(f,r); + return r; +} + +inline GREAL gim_sqrt(GREAL f) +{ + GREAL r; + GIM_SQRT(f,r); + return r; +} + +//! @} + +#endif // GIM_MATH_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp new file mode 100644 index 00000000000..1636eb7867c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp @@ -0,0 +1,135 @@ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + + +#include "gim_memory.h" +#include "stdlib.h" + +#ifdef GIM_SIMD_MEMORY +#include "LinearMath/btAlignedAllocator.h" +#endif + +static gim_alloc_function *g_allocfn = 0; +static gim_alloca_function *g_allocafn = 0; +static gim_realloc_function *g_reallocfn = 0; +static gim_free_function *g_freefn = 0; + +void gim_set_alloc_handler (gim_alloc_function *fn) +{ + g_allocfn = fn; +} + +void gim_set_alloca_handler (gim_alloca_function *fn) +{ + g_allocafn = fn; +} + +void gim_set_realloc_handler (gim_realloc_function *fn) +{ + g_reallocfn = fn; +} + +void gim_set_free_handler (gim_free_function *fn) +{ + g_freefn = fn; +} + +gim_alloc_function *gim_get_alloc_handler() +{ + return g_allocfn; +} + +gim_alloca_function *gim_get_alloca_handler() +{ + return g_allocafn; +} + + +gim_realloc_function *gim_get_realloc_handler () +{ + return g_reallocfn; +} + + +gim_free_function *gim_get_free_handler () +{ + return g_freefn; +} + + +void * gim_alloc(size_t size) +{ + void * ptr; + if (g_allocfn) + { + ptr = g_allocfn(size); + } + else + { +#ifdef GIM_SIMD_MEMORY + ptr = btAlignedAlloc(size,16); +#else + ptr = malloc(size); +#endif + } + return ptr; +} + +void * gim_alloca(size_t size) +{ + if (g_allocafn) return g_allocafn(size); else return gim_alloc(size); +} + + +void * gim_realloc(void *ptr, size_t oldsize, size_t newsize) +{ + void * newptr = gim_alloc(newsize); + size_t copysize = oldsize<newsize?oldsize:newsize; + gim_simd_memcpy(newptr,ptr,copysize); + gim_free(ptr); + return newptr; +} + +void gim_free(void *ptr) +{ + if (!ptr) return; + if (g_freefn) + { + g_freefn(ptr); + } + else + { + #ifdef GIM_SIMD_MEMORY + btAlignedFree(ptr); + #else + free(ptr); + #endif + } +} + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h new file mode 100644 index 00000000000..469a8280f7d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h @@ -0,0 +1,211 @@ +#ifndef GIM_MEMORY_H_INCLUDED +#define GIM_MEMORY_H_INCLUDED +/*! \file gim_memory.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + + +#include "gim_math.h" +#include <memory.h> + +//#define PREFETCH 1 +//! \defgroup PREFETCH +//! @{ +#ifdef PREFETCH +#include <xmmintrin.h> // for prefetch +#define pfval 64 +#define pfval2 128 +//! Prefetch 64 +#define pf(_x,_i) _mm_prefetch((void *)(_x + _i + pfval), 0) +//! Prefetch 128 +#define pf2(_x,_i) _mm_prefetch((void *)(_x + _i + pfval2), 0) +#else +//! Prefetch 64 +#define pf(_x,_i) +//! Prefetch 128 +#define pf2(_x,_i) +#endif +//! @} + +/*! \defgroup ARRAY_UTILITIES +\brief +Functions for manip packed arrays of numbers +*/ +//! @{ +#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\ +{\ + for (GUINT _i_=0;_i_<element_count ;++_i_)\ + {\ + dest_array[_i_] = source_array[_i_];\ + }\ +}\ + +#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\ +{\ + for (GUINT _i_=0;_i_<element_count ;++_i_)\ + {\ + copy_macro(dest_array[_i_],source_array[_i_]);\ + }\ +}\ + + +#define GIM_ZERO_ARRAY(array,element_count)\ +{\ + for (GUINT _i_=0;_i_<element_count ;++_i_)\ + {\ + array[_i_] = 0;\ + }\ +}\ + +#define GIM_CONSTANT_ARRAY(array,element_count,constant)\ +{\ + for (GUINT _i_=0;_i_<element_count ;++_i_)\ + {\ + array[_i_] = constant;\ + }\ +}\ +//! @} + +/*! \defgroup MEMORY_FUNCTION_PROTOTYPES +Function prototypes to allocate and free memory. +*/ +//! @{ +typedef void * gim_alloc_function (size_t size); +typedef void * gim_alloca_function (size_t size);//Allocs on the heap +typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize); +typedef void gim_free_function (void *ptr); +//! @} + +/*! \defgroup MEMORY_FUNCTION_HANDLERS +\brief +Memory Function Handlers + set new memory management functions. if fn is 0, the default handlers are + used. */ +//! @{ +void gim_set_alloc_handler (gim_alloc_function *fn); +void gim_set_alloca_handler (gim_alloca_function *fn); +void gim_set_realloc_handler (gim_realloc_function *fn); +void gim_set_free_handler (gim_free_function *fn); +//! @} + +/*! \defgroup MEMORY_FUNCTION_GET_HANDLERS +\brief +get current memory management functions. +*/ +//! @{ +gim_alloc_function *gim_get_alloc_handler (void); +gim_alloca_function *gim_get_alloca_handler(void); +gim_realloc_function *gim_get_realloc_handler (void); +gim_free_function *gim_get_free_handler (void); +//! @} + +/*! \defgroup MEMORY_FUNCTIONS +Standar Memory functions +*/ +//! @{ +void * gim_alloc(size_t size); +void * gim_alloca(size_t size); +void * gim_realloc(void *ptr, size_t oldsize, size_t newsize); +void gim_free(void *ptr); +//! @} + + +#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) + #define GIM_SIMD_MEMORY 1 +#endif + +//! SIMD POINTER INTEGER +#define SIMD_T GUINT64 +//! SIMD INTEGER SIZE +#define SIMD_T_SIZE sizeof(SIMD_T) + + +inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize) +{ +#ifdef GIM_SIMD_MEMORY +/* +//'long long int' is incompatible with visual studio 6... + //copy words + SIMD_T * ui_src_ptr = (SIMD_T *)src; + SIMD_T * ui_dst_ptr = (SIMD_T *)dst; + while(copysize>=SIMD_T_SIZE) + { + *(ui_dst_ptr++) = *(ui_src_ptr++); + copysize-=SIMD_T_SIZE; + } + if(copysize==0) return; +*/ + + char * c_src_ptr = (char *)src; + char * c_dst_ptr = (char *)dst; + while(copysize>0) + { + *(c_dst_ptr++) = *(c_src_ptr++); + copysize--; + } + return; +#else + memcpy(dst,src,copysize); +#endif +} + + + +template<class T> +inline void gim_swap_elements(T* _array,size_t _i,size_t _j) +{ + T _e_tmp_ = _array[_i]; + _array[_i] = _array[_j]; + _array[_j] = _e_tmp_; +} + + +template<class T> +inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j) +{ + char _e_tmp_[sizeof(T)]; + gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T)); + gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T)); + gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T)); +} + +template <int SIZE> +inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j) +{ + char _e_tmp_[SIZE]; + _i*=SIZE; + _j*=SIZE; + gim_simd_memcpy(_e_tmp_,_array+_i,SIZE); + gim_simd_memcpy(_array+_i,_array+_j,SIZE); + gim_simd_memcpy(_array+_j,_e_tmp_,SIZE); +} + +#endif // GIM_MEMORY_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h new file mode 100644 index 00000000000..9ecf61bdc6d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h @@ -0,0 +1,410 @@ +#ifndef GIM_RADIXSORT_H_INCLUDED +#define GIM_RADIXSORT_H_INCLUDED +/*! \file gim_radixsort.h +\author Francisco Len Nßjera. +Based on the work of Michael Herf : "fast floating-point radix sort" +Avaliable on http://www.stereopsis.com/radix.html +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "gim_memory.h" + +/*! \defgroup SORTING +\brief +Macros for sorting. +*/ + +//! Prototype for comparators +class less_comparator +{ + public: + + template<class T,class Z> + inline int operator() ( const T& a, const Z& b ) + { + return ( a<b?-1:(a>b?1:0)); + } +}; + +//! Prototype for comparators +class integer_comparator +{ + public: + + template<class T> + inline int operator() ( const T& a, const T& b ) + { + return (int)(a-b); + } +}; + +//!Prototype for getting the integer representation of an object +class uint_key_func +{ +public: + template<class T> + inline GUINT operator()( const T& a) + { + return (GUINT)a; + } +}; + + +//!Prototype for copying elements +class copy_elements_func +{ +public: + template<class T> + inline void operator()(T& a,T& b) + { + a = b; + } +}; + +//!Prototype for copying elements +class memcopy_elements_func +{ +public: + template<class T> + inline void operator()(T& a,T& b) + { + gim_simd_memcpy(&a,&b,sizeof(T)); + } +}; + + +//! @{ +struct GIM_RSORT_TOKEN +{ + GUINT m_key; + GUINT m_value; + GIM_RSORT_TOKEN() + { + } + GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken) + { + m_key = rtoken.m_key; + m_value = rtoken.m_value; + } + + inline bool operator <(const GIM_RSORT_TOKEN& other) const + { + return (m_key < other.m_key); + } + + inline bool operator >(const GIM_RSORT_TOKEN& other) const + { + return (m_key > other.m_key); + } +}; + +//! Prototype for comparators +class GIM_RSORT_TOKEN_COMPARATOR +{ + public: + + inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b ) + { + return (int)((a.m_key) - (b.m_key)); + } +}; + + + +#define kHist 2048 +// ---- utils for accessing 11-bit quantities +#define D11_0(x) (x & 0x7FF) +#define D11_1(x) (x >> 11 & 0x7FF) +#define D11_2(x) (x >> 22 ) + + + +///Radix sort for unsigned integer keys +inline void gim_radix_sort_rtokens( + GIM_RSORT_TOKEN * array, + GIM_RSORT_TOKEN * sorted, GUINT element_count) +{ + GUINT i; + GUINT b0[kHist * 3]; + GUINT *b1 = b0 + kHist; + GUINT *b2 = b1 + kHist; + for (i = 0; i < kHist * 3; ++i) + { + b0[i] = 0; + } + GUINT fi; + GUINT pos; + for (i = 0; i < element_count; ++i) + { + fi = array[i].m_key; + b0[D11_0(fi)] ++; + b1[D11_1(fi)] ++; + b2[D11_2(fi)] ++; + } + { + GUINT sum0 = 0, sum1 = 0, sum2 = 0; + GUINT tsum; + for (i = 0; i < kHist; ++i) + { + tsum = b0[i] + sum0; + b0[i] = sum0 - 1; + sum0 = tsum; + tsum = b1[i] + sum1; + b1[i] = sum1 - 1; + sum1 = tsum; + tsum = b2[i] + sum2; + b2[i] = sum2 - 1; + sum2 = tsum; + } + } + for (i = 0; i < element_count; ++i) + { + fi = array[i].m_key; + pos = D11_0(fi); + pos = ++b0[pos]; + sorted[pos].m_key = array[i].m_key; + sorted[pos].m_value = array[i].m_value; + } + for (i = 0; i < element_count; ++i) + { + fi = sorted[i].m_key; + pos = D11_1(fi); + pos = ++b1[pos]; + array[pos].m_key = sorted[i].m_key; + array[pos].m_value = sorted[i].m_value; + } + for (i = 0; i < element_count; ++i) + { + fi = array[i].m_key; + pos = D11_2(fi); + pos = ++b2[pos]; + sorted[pos].m_key = array[i].m_key; + sorted[pos].m_value = array[i].m_value; + } +} + + + + +/// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN +/*! +*\param array Array of elements to sort +*\param sorted_tokens Tokens of sorted elements +*\param element_count element count +*\param uintkey_macro Functor which retrieves the integer representation of an array element +*/ +template<typename T, class GETKEY_CLASS> +void gim_radix_sort_array_tokens( + T* array , + GIM_RSORT_TOKEN * sorted_tokens, + GUINT element_count,GETKEY_CLASS uintkey_macro) +{ + GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count); + for (GUINT _i=0;_i<element_count;++_i) + { + _unsorted[_i].m_key = uintkey_macro(array[_i]); + _unsorted[_i].m_value = _i; + } + gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count); + gim_free(_unsorted); + gim_free(_unsorted); +} + +/// Sorts array in place. For generic use +/*! +\param type Type of the array +\param array +\param element_count +\param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY +\param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS +*/ +template<typename T, class GETKEY_CLASS, class COPY_CLASS> +void gim_radix_sort( + T * array, GUINT element_count, + GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro) +{ + GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count); + gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro); + T * _original_array = (T *) gim_alloc(sizeof(T)*element_count); + gim_simd_memcpy(_original_array,array,sizeof(T)*element_count); + for (GUINT _i=0;_i<element_count;++_i) + { + copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]); + } + gim_free(_original_array); + gim_free(_sorted); +} + +//! Failsafe Iterative binary search, +/*! +If the element is not found, it returns the nearest upper element position, may be the further position after the last element. +\param _array +\param _start_i the beginning of the array +\param _end_i the ending index of the array +\param _search_key Value to find +\param _comp_macro macro for comparing elements +\param _found If true the value has found. Boolean +\param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value +*/ +template<class T, typename KEYCLASS, typename COMP_CLASS> +bool gim_binary_search_ex( + const T* _array, GUINT _start_i, + GUINT _end_i,GUINT & _result_index, + const KEYCLASS & _search_key, + COMP_CLASS _comp_macro) +{ + GUINT _k; + int _comp_result; + GUINT _i = _start_i; + GUINT _j = _end_i+1; + while (_i < _j) + { + _k = (_j+_i-1)/2; + _comp_result = _comp_macro(_array[_k], _search_key); + if (_comp_result == 0) + { + _result_index = _k; + return true; + } + else if (_comp_result < 0) + { + _i = _k+1; + } + else + { + _j = _k; + } + } + _result_index = _i; + return false; +} + + + +//! Failsafe Iterative binary search,Template version +/*! +If the element is not found, it returns the nearest upper element position, may be the further position after the last element. +\param _array +\param _start_i the beginning of the array +\param _end_i the ending index of the array +\param _search_key Value to find +\param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value +\return true if found, else false +*/ +template<class T> +bool gim_binary_search( + const T*_array,GUINT _start_i, + GUINT _end_i,const T & _search_key, + GUINT & _result_index) +{ + GUINT _i = _start_i; + GUINT _j = _end_i+1; + GUINT _k; + while(_i < _j) + { + _k = (_j+_i-1)/2; + if(_array[_k]==_search_key) + { + _result_index = _k; + return true; + } + else if (_array[_k]<_search_key) + { + _i = _k+1; + } + else + { + _j = _k; + } + } + _result_index = _i; + return false; +} + + + +///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ +template <typename T, typename COMP_CLASS> +void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS 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 < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0) + { + child++; + } + /* pick larger child */ + if (CompareFunc(temp , pArr[child - 1])<0) + { + /* move child up */ + pArr[k - 1] = pArr[child - 1]; + k = child; + } + else + { + break; + } + } + pArr[k - 1] = temp; +} /*downHeap*/ + + +template <typename T, typename COMP_CLASS> +void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc) +{ + /* sort a[0..N-1], N.B. 0 to N-1 */ + GUINT k; + GUINT n = element_count; + for (k = n/2; k > 0; k--) + { + gim_down_heap(pArr, k, n, CompareFunc); + } + + /* a[1..N] is now a heap */ + while ( n>=2 ) + { + gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */ + --n; + /* restore a[1..i-1] heap */ + gim_down_heap(pArr, 1, n, CompareFunc); + } +} + + + +//! @} +#endif // GIM_RADIXSORT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp new file mode 100644 index 00000000000..ef6feabdfee --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -0,0 +1,640 @@ + +/*! \file gim_tri_collision.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "gim_tri_collision.h" + + +#define TRI_LOCAL_EPSILON 0.000001f +#define MIN_EDGE_EDGE_DIS 0.00001f + + +class _GIM_TRIANGLE_CALCULATION_CACHE +{ +public: + GREAL margin; + btVector3 tu_vertices[3]; + btVector3 tv_vertices[3]; + btVector4 tu_plane; + btVector4 tv_plane; + btVector3 closest_point_u; + btVector3 closest_point_v; + btVector3 edge_edge_dir; + btVector3 distances; + GREAL du[4]; + GREAL du0du1; + GREAL du0du2; + GREAL dv[4]; + GREAL dv0dv1; + GREAL dv0dv2; + btVector3 temp_points[MAX_TRI_CLIPPING]; + btVector3 temp_points1[MAX_TRI_CLIPPING]; + btVector3 contact_points[MAX_TRI_CLIPPING]; + + + + //! if returns false, the faces are paralele + SIMD_FORCE_INLINE bool compute_intervals( + const GREAL &D0, + const GREAL &D1, + const GREAL &D2, + const GREAL &D0D1, + const GREAL &D0D2, + GREAL & scale_edge0, + GREAL & scale_edge1, + GUINT &edge_index0, + GUINT &edge_index1) + { + if(D0D1>0.0f) + { + /* here we know that D0D2<=0.0 */ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ + scale_edge0 = -D2/(D0-D2); + scale_edge1 = -D1/(D2-D1); + edge_index0 = 2;edge_index1 = 1; + } + else if(D0D2>0.0f) + { + /* here we know that d0d1<=0.0 */ + scale_edge0 = -D0/(D1-D0); + scale_edge1 = -D1/(D2-D1); + edge_index0 = 0;edge_index1 = 1; + } + else if(D1*D2>0.0f || D0!=0.0f) + { + /* here we know that d0d1<=0.0 or that D0!=0.0 */ + scale_edge0 = -D0/(D1-D0); + scale_edge1 = -D2/(D0-D2); + edge_index0 = 0 ;edge_index1 = 2; + } + else + { + return false; + } + return true; + } + + + //! clip triangle + /*! + */ + SIMD_FORCE_INLINE GUINT clip_triangle( + const btVector4 & tri_plane, + const btVector3 * tripoints, + const btVector3 * srcpoints, + btVector3 * clip_points) + { + // edge 0 + + btVector4 edgeplane; + + EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane); + + GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( + edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); + + if(clipped_count == 0) return 0; + + // edge 1 + + EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); + + clipped_count = PLANE_CLIP_POLYGON3D( + edgeplane,temp_points,clipped_count,temp_points1); + + if(clipped_count == 0) return 0; + + // edge 2 + + EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); + + clipped_count = PLANE_CLIP_POLYGON3D( + edgeplane,temp_points1,clipped_count,clip_points); + + return clipped_count; + + + /*GUINT i0 = (tri_plane.closestAxis()+1)%3; + GUINT i1 = (i0+1)%3; + // edge 0 + btVector3 temp_points[MAX_TRI_CLIPPING]; + btVector3 temp_points1[MAX_TRI_CLIPPING]; + + GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC( + 0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points, + DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1)); + + + if(clipped_count == 0) return 0; + + // edge 1 + clipped_count = PLANE_CLIP_POLYGON_GENERIC( + 0,temp_points,clipped_count,temp_points1, + DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1)); + + if(clipped_count == 0) return 0; + + // edge 2 + clipped_count = PLANE_CLIP_POLYGON_GENERIC( + 0,temp_points1,clipped_count,clipped_points, + DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1)); + + return clipped_count;*/ + } + + SIMD_FORCE_INLINE void sort_isect( + GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1) + { + if(isect1<isect0) + { + //swap + GIM_SWAP_NUMBERS(isect0,isect1); + GIM_SWAP_NUMBERS(e0,e1); + btVector3 tmp = vec0; + vec0 = vec1; + vec1 = tmp; + } + } + + //! Test verifying interval intersection with the direction between planes + /*! + \pre tv_plane and tu_plane must be set + \post + distances[2] is set with the distance + closest_point_u, closest_point_v, edge_edge_dir are set too + \return + - 0: faces are paralele + - 1: face U casts face V + - 2: face V casts face U + - 3: nearest edges + */ + SIMD_FORCE_INLINE GUINT cross_line_intersection_test() + { + // Compute direction of intersection line + edge_edge_dir = tu_plane.cross(tv_plane); + GREAL Dlen; + VEC_LENGTH(edge_edge_dir,Dlen); + + if(Dlen<0.0001) + { + return 0; //faces near paralele + } + + edge_edge_dir*= 1/Dlen;//normalize + + + // Compute interval for triangle 1 + GUINT tu_e0,tu_e1;//edge indices + GREAL tu_scale_e0,tu_scale_e1;//edge scale + if(!compute_intervals(du[0],du[1],du[2], + du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0; + + // Compute interval for triangle 2 + GUINT tv_e0,tv_e1;//edge indices + GREAL tv_scale_e0,tv_scale_e1;//edge scale + + if(!compute_intervals(dv[0],dv[1],dv[2], + dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0; + + //proyected vertices + btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0); + btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1); + + btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0); + btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1); + + //proyected intervals + GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)}; + GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)}; + + sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1); + sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1); + + const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint + const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint + + if(midpoint_u<midpoint_v) + { + if(isect_u[1]>=isect_v[1]) // face U casts face V + { + return 1; + } + else if(isect_v[0]<=isect_u[0]) // face V casts face U + { + return 2; + } + // closest points + closest_point_u = up_e1; + closest_point_v = vp_e0; + // calc edges and separation + + if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead + { + SEGMENT_COLLISION( + tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3], + tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3], + closest_point_u, + closest_point_v); + + edge_edge_dir = closest_point_u-closest_point_v; + VEC_LENGTH(edge_edge_dir,distances[2]); + edge_edge_dir *= 1.0f/distances[2];// normalize + } + else + { + distances[2] = isect_v[0]-isect_u[1];//distance negative + //edge_edge_dir *= -1.0f; //normal pointing from V to U + } + + } + else + { + if(isect_v[1]>=isect_u[1]) // face V casts face U + { + return 2; + } + else if(isect_u[0]<=isect_v[0]) // face U casts face V + { + return 1; + } + // closest points + closest_point_u = up_e0; + closest_point_v = vp_e1; + // calc edges and separation + + if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead + { + SEGMENT_COLLISION( + tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3], + tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3], + closest_point_u, + closest_point_v); + + edge_edge_dir = closest_point_u-closest_point_v; + VEC_LENGTH(edge_edge_dir,distances[2]); + edge_edge_dir *= 1.0f/distances[2];// normalize + } + else + { + distances[2] = isect_u[0]-isect_v[1];//distance negative + //edge_edge_dir *= -1.0f; //normal pointing from V to U + } + } + return 3; + } + + + //! collides by two sides + SIMD_FORCE_INLINE bool triangle_collision( + const btVector3 & u0, + const btVector3 & u1, + const btVector3 & u2, + GREAL margin_u, + const btVector3 & v0, + const btVector3 & v1, + const btVector3 & v2, + GREAL margin_v, + GIM_TRIANGLE_CONTACT_DATA & contacts) + { + + margin = margin_u + margin_v; + + tu_vertices[0] = u0; + tu_vertices[1] = u1; + tu_vertices[2] = u2; + + tv_vertices[0] = v0; + tv_vertices[1] = v1; + tv_vertices[2] = v2; + + //create planes + // plane v vs U points + + TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane); + + du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]); + du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]); + du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]); + + + du0du1 = du[0] * du[1]; + du0du2 = du[0] * du[2]; + + + if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? + { + if(du[0]<0) //we need test behind the triangle plane + { + distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = -distances[0]; + if(distances[0]>margin) return false; //never intersect + + //reorder triangle v + VEC_SWAP(tv_vertices[0],tv_vertices[1]); + VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + } + else + { + distances[0] = GIM_MIN3(du[0],du[1],du[2]); + if(distances[0]>margin) return false; //never intersect + } + } + else + { + //Look if we need to invert the triangle + distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid + + if(distances[0]<0.0f) + { + //reorder triangle v + VEC_SWAP(tv_vertices[0],tv_vertices[1]); + VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + + distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = -distances[0]; + } + else + { + distances[0] = GIM_MIN3(du[0],du[1],du[2]); + } + } + + + // plane U vs V points + + TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane); + + dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]); + dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]); + dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]); + + dv0dv1 = dv[0] * dv[1]; + dv0dv2 = dv[0] * dv[2]; + + + if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? + { + if(dv[0]<0) //we need test behind the triangle plane + { + distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = -distances[1]; + if(distances[1]>margin) return false; //never intersect + + //reorder triangle u + VEC_SWAP(tu_vertices[0],tu_vertices[1]); + VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + } + else + { + distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); + if(distances[1]>margin) return false; //never intersect + } + } + else + { + //Look if we need to invert the triangle + distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid + + if(distances[1]<0.0f) + { + //reorder triangle v + VEC_SWAP(tu_vertices[0],tu_vertices[1]); + VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + + distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = -distances[1]; + } + else + { + distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); + } + } + + GUINT bl; + /* bl = cross_line_intersection_test(); + if(bl==3) + { + //take edge direction too + bl = distances.maxAxis(); + } + else + {*/ + bl = 0; + if(distances[0]<distances[1]) bl = 1; + //} + + if(bl==2) //edge edge separation + { + if(distances[2]>margin) return false; + + contacts.m_penetration_depth = -distances[2] + margin; + contacts.m_points[0] = closest_point_v; + contacts.m_point_count = 1; + VEC_COPY(contacts.m_separating_normal,edge_edge_dir); + + return true; + } + + //clip face against other + + + GUINT point_count; + //TODO + if(bl == 0) //clip U points against V + { + point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points); + if(point_count == 0) return false; + contacts.merge_points(tv_plane,margin,contact_points,point_count); + } + else //clip V points against U + { + point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points); + if(point_count == 0) return false; + contacts.merge_points(tu_plane,margin,contact_points,point_count); + contacts.m_separating_normal *= -1.f; + } + if(contacts.m_point_count == 0) return false; + return true; + } + +}; + + +/*class _GIM_TRIANGLE_CALCULATION_CACHE +{ +public: + GREAL margin; + GUINT clipped_count; + btVector3 tu_vertices[3]; + btVector3 tv_vertices[3]; + btVector3 temp_points[MAX_TRI_CLIPPING]; + btVector3 temp_points1[MAX_TRI_CLIPPING]; + btVector3 clipped_points[MAX_TRI_CLIPPING]; + GIM_TRIANGLE_CONTACT_DATA contacts1; + GIM_TRIANGLE_CONTACT_DATA contacts2; + + + //! clip triangle + GUINT clip_triangle( + const btVector4 & tri_plane, + const btVector3 * tripoints, + const btVector3 * srcpoints, + btVector3 * clipped_points) + { + // edge 0 + + btVector4 edgeplane; + + EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane); + + GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( + edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); + + if(clipped_count == 0) return 0; + + // edge 1 + + EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); + + clipped_count = PLANE_CLIP_POLYGON3D( + edgeplane,temp_points,clipped_count,temp_points1); + + if(clipped_count == 0) return 0; + + // edge 2 + + EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); + + clipped_count = PLANE_CLIP_POLYGON3D( + edgeplane,temp_points1,clipped_count,clipped_points); + + return clipped_count; + } + + + + + //! collides only on one side + bool triangle_collision( + const btVector3 & u0, + const btVector3 & u1, + const btVector3 & u2, + GREAL margin_u, + const btVector3 & v0, + const btVector3 & v1, + const btVector3 & v2, + GREAL margin_v, + GIM_TRIANGLE_CONTACT_DATA & contacts) + { + + margin = margin_u + margin_v; + + + tu_vertices[0] = u0; + tu_vertices[1] = u1; + tu_vertices[2] = u2; + + tv_vertices[0] = v0; + tv_vertices[1] = v1; + tv_vertices[2] = v2; + + //create planes + // plane v vs U points + + + TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal); + + clipped_count = clip_triangle( + contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points); + + if(clipped_count == 0 ) + { + return false;//Reject + } + + //find most deep interval face1 + contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count); + if(contacts1.m_point_count == 0) return false; // too far + + //Normal pointing to triangle1 + //contacts1.m_separating_normal *= -1.f; + + //Clip tri1 by tri2 edges + + TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal); + + clipped_count = clip_triangle( + contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points); + + if(clipped_count == 0 ) + { + return false;//Reject + } + + //find most deep interval face1 + contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count); + if(contacts2.m_point_count == 0) return false; // too far + + contacts2.m_separating_normal *= -1.f; + + ////check most dir for contacts + if(contacts2.m_penetration_depth<contacts1.m_penetration_depth) + { + contacts.copy_from(contacts2); + } + else + { + contacts.copy_from(contacts1); + } + return true; + } + + +};*/ + + + +bool GIM_TRIANGLE::collide_triangle_hard_test( + const GIM_TRIANGLE & other, + GIM_TRIANGLE_CONTACT_DATA & contact_data) const +{ + _GIM_TRIANGLE_CALCULATION_CACHE calc_cache; + return calc_cache.triangle_collision( + m_vertices[0],m_vertices[1],m_vertices[2],m_margin, + other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin, + contact_data); + +} + + + + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h new file mode 100644 index 00000000000..7cea8fc33a6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -0,0 +1,383 @@ +#ifndef GIM_TRI_COLLISION_H_INCLUDED +#define GIM_TRI_COLLISION_H_INCLUDED + +/*! \file gim_tri_collision.h +\author Francisco Len Nßjera +*/ +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + (3) The zlib/libpng license that is included with this library in + the file GIMPACT-LICENSE-ZLIB.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "gim_box_collision.h" +#include "gim_clip_polygon.h" + +/*! \addtogroup GEOMETRIC_OPERATIONS +*/ +//! @{ + + + +#define MAX_TRI_CLIPPING 16 + +//! Structure for collision +struct GIM_TRIANGLE_CONTACT_DATA +{ + GREAL m_penetration_depth; + GUINT m_point_count; + btVector4 m_separating_normal; + btVector3 m_points[MAX_TRI_CLIPPING]; + + SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other) + { + m_penetration_depth = other.m_penetration_depth; + m_separating_normal = other.m_separating_normal; + m_point_count = other.m_point_count; + GUINT i = m_point_count; + while(i--) + { + m_points[i] = other.m_points[i]; + } + } + + GIM_TRIANGLE_CONTACT_DATA() + { + } + + GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other) + { + copy_from(other); + } + + + + + //! classify points that are closer + template<typename DISTANCE_FUNC,typename CLASS_PLANE> + SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane, + GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func) + { + m_point_count = 0; + m_penetration_depth= -1000.0f; + + GUINT point_indices[MAX_TRI_CLIPPING]; + + GUINT _k; + + for(_k=0;_k<point_count;_k++) + { + GREAL _dist = -distance_func(plane,points[_k]) + margin; + + if(_dist>=0.0f) + { + if(_dist>m_penetration_depth) + { + m_penetration_depth = _dist; + point_indices[0] = _k; + m_point_count=1; + } + else if((_dist+G_EPSILON)>=m_penetration_depth) + { + point_indices[m_point_count] = _k; + m_point_count++; + } + } + } + + for( _k=0;_k<m_point_count;_k++) + { + m_points[_k] = points[point_indices[_k]]; + } + } + + //! classify points that are closer + SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin, + const btVector3 * points, GUINT point_count) + { + m_separating_normal = plane; + mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC()); + } +}; + + +//! Class for colliding triangles +class GIM_TRIANGLE +{ +public: + btScalar m_margin; + btVector3 m_vertices[3]; + + GIM_TRIANGLE():m_margin(0.1f) + { + } + + SIMD_FORCE_INLINE GIM_AABB get_box() const + { + return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); + } + + SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const + { + TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal); + } + + SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const + { + TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);; + } + + SIMD_FORCE_INLINE void apply_transform(const btTransform & trans) + { + m_vertices[0] = trans(m_vertices[0]); + m_vertices[1] = trans(m_vertices[1]); + m_vertices[2] = trans(m_vertices[2]); + } + + SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane) const + { + const btVector3 & e0 = m_vertices[edge_index]; + const btVector3 & e1 = m_vertices[(edge_index+1)%3]; + EDGE_PLANE(e0,e1,triangle_normal,plane); + } + + //! Gets the relative transformation of this triangle + /*! + The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0: + - triangle normal corresponds to Z axis. + - 1st normalized edge corresponds to X axis, + + */ + SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform) const + { + btMatrix3x3 & matrix = triangle_transform.getBasis(); + + btVector3 zaxis; + get_normal(zaxis); + MAT_SET_Z(matrix,zaxis); + + btVector3 xaxis = m_vertices[1] - m_vertices[0]; + VEC_NORMALIZE(xaxis); + MAT_SET_X(matrix,xaxis); + + //y axis + xaxis = zaxis.cross(xaxis); + MAT_SET_Y(matrix,xaxis); + + triangle_transform.setOrigin(m_vertices[0]); + } + + + //! Test triangles by finding separating axis + /*! + \param other Triangle for collide + \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle + */ + bool collide_triangle_hard_test( + const GIM_TRIANGLE & other, + GIM_TRIANGLE_CONTACT_DATA & contact_data) const; + + //! Test boxes before doing hard test + /*! + \param other Triangle for collide + \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle + \ + */ + SIMD_FORCE_INLINE bool collide_triangle( + const GIM_TRIANGLE & other, + GIM_TRIANGLE_CONTACT_DATA & contact_data) const + { + //test box collisioin + GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); + GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin); + if(!boxu.has_collision(boxv)) return false; + + //do hard test + return collide_triangle_hard_test(other,contact_data); + } + + /*! + + Solve the System for u,v parameters: + + u*axe1[i1] + v*axe2[i1] = vecproj[i1] + u*axe1[i2] + v*axe2[i2] = vecproj[i2] + + sustitute: + v = (vecproj[i2] - u*axe1[i2])/axe2[i2] + + then the first equation in terms of 'u': + + --> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1] + + --> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1] + + --> u*(axe1[i1] - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2] + + --> u*((axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2] + + --> u*(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1] + + --> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) + +if 0.0<= u+v <=1.0 then they are inside of triangle + + \return false if the point is outside of triangle.This function doesn't take the margin + */ + SIMD_FORCE_INLINE bool get_uv_parameters( + const btVector3 & point, + const btVector3 & tri_plane, + GREAL & u, GREAL & v) const + { + btVector3 _axe1 = m_vertices[1]-m_vertices[0]; + btVector3 _axe2 = m_vertices[2]-m_vertices[0]; + btVector3 _vecproj = point - m_vertices[0]; + GUINT _i1 = (tri_plane.closestAxis()+1)%3; + GUINT _i2 = (_i1+1)%3; + if(btFabs(_axe2[_i2])<G_EPSILON) + { + u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1] - _axe1[_i1]*_axe2[_i2]); + v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1]; + } + else + { + u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2] - _axe1[_i2]*_axe2[_i1]); + v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2]; + } + + if(u<-G_EPSILON) + { + return false; + } + else if(v<-G_EPSILON) + { + return false; + } + else + { + float sumuv; + sumuv = u+v; + if(sumuv<-G_EPSILON) + { + return false; + } + else if(sumuv-1.0f>G_EPSILON) + { + return false; + } + } + return true; + } + + //! is point in triangle beam? + /*! + Test if point is in triangle, with m_margin tolerance + */ + SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const + { + //Test with edge 0 + btVector4 edge_plane; + this->get_edge_plane(0,tri_normal,edge_plane); + GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point); + if(dist-m_margin>0.0f) return false; // outside plane + + this->get_edge_plane(1,tri_normal,edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane,point); + if(dist-m_margin>0.0f) return false; // outside plane + + this->get_edge_plane(2,tri_normal,edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane,point); + if(dist-m_margin>0.0f) return false; // outside plane + return true; + } + + + //! Bidireccional ray collision + SIMD_FORCE_INLINE bool ray_collision( + const btVector3 & vPoint, + const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, + GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + { + btVector4 faceplane; + { + btVector3 dif1 = m_vertices[1] - m_vertices[0]; + btVector3 dif2 = m_vertices[2] - m_vertices[0]; + VEC_CROSS(faceplane,dif1,dif2); + faceplane[3] = m_vertices[0].dot(faceplane); + } + + GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); + if(res == 0) return false; + if(! is_point_inside(pout,faceplane)) return false; + + if(res==2) //invert normal + { + triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); + } + else + { + triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + } + + VEC_NORMALIZE(triangle_normal); + + return true; + } + + + //! one direccion ray collision + SIMD_FORCE_INLINE bool ray_collision_front_side( + const btVector3 & vPoint, + const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, + GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + { + btVector4 faceplane; + { + btVector3 dif1 = m_vertices[1] - m_vertices[0]; + btVector3 dif2 = m_vertices[2] - m_vertices[0]; + VEC_CROSS(faceplane,dif1,dif2); + faceplane[3] = m_vertices[0].dot(faceplane); + } + + GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); + if(res != 1) return false; + + if(!is_point_inside(pout,faceplane)) return false; + + triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + + VEC_NORMALIZE(triangle_normal); + + return true; + } + +}; + + + +//! @} + +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 2c565734e97..6551cfb92fe 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -16,7 +16,6 @@ subject to the following restrictions: #include "btContinuousConvexCollision.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "LinearMath/btTransformUtil.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -26,7 +25,7 @@ subject to the following restrictions: -btContinuousConvexCollision::btContinuousConvexCollision ( btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) +btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) :m_simplexSolver(simplexSolver), m_penetrationDepthSolver(penetrationDepthSolver), m_convexA(convexA),m_convexB(convexB) @@ -35,7 +34,7 @@ m_convexA(convexA),m_convexB(convexB) /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. /// You don't want your game ever to lock-up. -#define MAX_ITERATIONS 1000 +#define MAX_ITERATIONS 64 bool btContinuousConvexCollision::calcTimeOfImpact( const btTransform& fromA, @@ -52,10 +51,18 @@ bool btContinuousConvexCollision::calcTimeOfImpact( 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; + btVector3 relLinVel = (linVelB-linVelA); + + btScalar relLinVelocLength = (linVelB-linVelA).length(); + + if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) + return false; + btScalar radius = btScalar(0.001); @@ -93,7 +100,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD - gjk.setIgnoreMargin(true); + // gjk.setIgnoreMargin(true); input.m_transformA = fromA; input.m_transformB = fromB; @@ -108,25 +115,31 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btScalar dist; dist = pointCollector1.m_distance; n = pointCollector1.m_normalOnBInWorld; + + btScalar projectedLinearVelocity = relLinVel.dot(n); //not close enough while (dist > radius) { numIter++; if (numIter > maxIter) + { return false; //todo: report a failure - + } btScalar dLambda = btScalar(0.); + projectedLinearVelocity = relLinVel.dot(n); + //calculate safe moving fraction from distance / (linear+rotational velocity) //btScalar clippedDist = GEN_min(angularConservativeRadius,dist); //btScalar clippedDist = dist; - btScalar projectedLinearVelocity = (linVelB-linVelA).dot(n); dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); + + lambda = lambda + dLambda; if (lambda > btScalar(1.)) @@ -135,9 +148,14 @@ bool btContinuousConvexCollision::calcTimeOfImpact( if (lambda < btScalar(0.)) return false; + //todo: next check with relative epsilon if (lambda <= lastLambda) + { + return false; + //n.setValue(0,0,0); break; + } lastLambda = lambda; @@ -163,11 +181,13 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { //degenerate ?! result.m_fraction = lastLambda; - result.m_normal = n; + n = pointCollector.m_normalOnBInWorld; + result.m_normal=n;//.setValue(1,1,1);// = n; + result.m_hitPoint = pointCollector.m_pointInWorld; return true; } c = pointCollector.m_pointInWorld; - + n = pointCollector.m_normalOnBInWorld; dist = pointCollector.m_distance; } else { @@ -177,8 +197,13 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON) + return false; + result.m_fraction = lambda; result.m_normal = n; + result.m_hitPoint = c; return true; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h index 9901bab4b45..28c2b4d6156 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -30,13 +30,13 @@ class btContinuousConvexCollision : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_penetrationDepthSolver; - btConvexShape* m_convexA; - btConvexShape* m_convexB; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - btContinuousConvexCollision (btConvexShape* shapeA,btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + btContinuousConvexCollision (const btConvexShape* shapeA,const btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual bool calcTimeOfImpact( const btTransform& fromA, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index 3101b59993d..0edf4dcd496 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 @@ -42,19 +42,21 @@ public: CastResult() :m_fraction(btScalar(1e30)), - m_debugDrawer(0) + m_debugDrawer(0), + m_allowedPenetration(btScalar(0)) { } virtual ~CastResult() {}; - btVector3 m_normal; - btScalar m_fraction; btTransform m_hitTransformA; btTransform m_hitTransformB; - + btVector3 m_normal; + btVector3 m_hitPoint; + btScalar m_fraction; //input and output btIDebugDraw* m_debugDrawer; + btScalar m_allowedPenetration; }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 7caeba4be45..99690921317 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. @@ -31,7 +31,7 @@ public: virtual ~btConvexPenetrationDepthSolver() {}; virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - btConvexShape* convexA,btConvexShape* convexB, + const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btPoint3& pa, btPoint3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index 15000c1ab61..db797d5141f 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 @@ -58,7 +58,7 @@ struct btDiscreteCollisionDetectorInterface // give either closest points (distance > 0) or penetration (distance) // the normal always points from B towards A // - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) = 0; + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index 93edffeafd6..bef697a0a11 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -17,12 +17,17 @@ subject to the following restrictions: #include "btGjkConvexCast.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "btGjkPairDetector.h" #include "btPointCollector.h" +#include "LinearMath/btTransformUtil.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_ITERATIONS 64 +#else +#define MAX_ITERATIONS 32 +#endif -btGjkConvexCast::btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) +btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) :m_simplexSolver(simplexSolver), m_convexA(convexA), m_convexB(convexB) @@ -38,120 +43,113 @@ bool btGjkConvexCast::calcTimeOfImpact( { - btMinkowskiSumShape combi(m_convexA,m_convexB); - btMinkowskiSumShape* convex = &combi; - - btTransform rayFromLocalA; - btTransform rayToLocalA; - - rayFromLocalA = fromA.inverse()* fromB; - rayToLocalA = toA.inverse()* toB; - - - btTransform trA,trB; - trA = btTransform(fromA); - trB = btTransform(fromB); - trA.setOrigin(btPoint3(0,0,0)); - trB.setOrigin(btPoint3(0,0,0)); - - convex->setTransformA(trA); - convex->setTransformB(trB); - + m_simplexSolver->reset(); + /// compute linear velocity for this interval, to interpolate + //assume no rotation/angular velocity, assert here? + btVector3 linVelA,linVelB; + linVelA = toA.getOrigin()-fromA.getOrigin(); + linVelB = toB.getOrigin()-fromB.getOrigin(); + btScalar radius = btScalar(0.001); + btScalar lambda = btScalar(0.); + btVector3 v(1,0,0); - btScalar radius = btScalar(0.01); + int maxIter = MAX_ITERATIONS; - btScalar lambda = btScalar(0.); - btVector3 s = rayFromLocalA.getOrigin(); - btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin(); - btVector3 x = s; btVector3 n; - n.setValue(0,0,0); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; + btVector3 r = (linVelA-linVelB); btScalar lastLambda = lambda; + //btScalar epsilon = btScalar(0.001); + int numIter = 0; //first solution, using GJK - //no penetration support for now, perhaps pass a pointer when we really want it - btConvexPenetrationDepthSolver* penSolverPtr = 0; btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(btScalar(0.0)); - raySphere.setMargin(btScalar(0.)); - btTransform sphereTr; - sphereTr.setIdentity(); - sphereTr.setOrigin( rayFromLocalA.getOrigin()); +// result.drawCoordSystem(sphereTr); - result.drawCoordSystem(sphereTr); - { - btPointCollector pointCollector1; - btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); + btPointCollector pointCollector; - btGjkPairDetector::ClosestPointInput input; - input.m_transformA = sphereTr; - input.m_transformB = identityTrans; - gjk.getClosestPoints(input,pointCollector1,0); + + btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver); + btGjkPairDetector::ClosestPointInput input; - hasResult = pointCollector1.m_hasResult; - c = pointCollector1.m_pointInWorld; - n = pointCollector1.m_normalOnBInWorld; - } + //we don't use margins during CCD + // gjk.setIgnoreMargin(true); - + input.m_transformA = fromA; + input.m_transformB = fromB; + gjk.getClosestPoints(input,pointCollector,0); + + hasResult = pointCollector.m_hasResult; + c = pointCollector.m_pointInWorld; if (hasResult) { btScalar dist; - dist = (c-x).length(); - if (dist < radius) - { - //penetration - lastLambda = btScalar(1.); - } + dist = pointCollector.m_distance; + n = pointCollector.m_normalOnBInWorld; + + //not close enough while (dist > radius) { + numIter++; + if (numIter > maxIter) + { + return false; //todo: report a failure + } + btScalar dLambda = btScalar(0.); + + btScalar projectedLinearVelocity = r.dot(n); - n = x - c; - btScalar nDotr = n.dot(r); + dLambda = dist / (projectedLinearVelocity); + + lambda = lambda - dLambda; - if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON)) + if (lambda > btScalar(1.)) return false; - - lambda = lambda - n.dot(n) / nDotr; + + if (lambda < btScalar(0.)) + return false; + + //todo: next check with relative epsilon if (lambda <= lastLambda) + { + return false; + //n.setValue(0,0,0); break; - + } lastLambda = lambda; - x = s + lambda * r; - - sphereTr.setOrigin( x ); - result.drawCoordSystem(sphereTr); - btPointCollector pointCollector; - btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); - btGjkPairDetector::ClosestPointInput input; - input.m_transformA = sphereTr; - input.m_transformB = identityTrans; + //interpolate to next lambda + result.DebugDraw( lambda ); + input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); + input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); + gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { if (pointCollector.m_distance < btScalar(0.)) { - //degeneracy, report a hit result.m_fraction = lastLambda; - result.m_normal = n; + n = pointCollector.m_normalOnBInWorld; + result.m_normal=n; + result.m_hitPoint = pointCollector.m_pointInWorld; return true; } - c = pointCollector.m_pointInWorld; - dist = (c-x).length(); + c = pointCollector.m_pointInWorld; + n = pointCollector.m_normalOnBInWorld; + dist = pointCollector.m_distance; } else { //?? @@ -160,15 +158,19 @@ bool btGjkConvexCast::calcTimeOfImpact( } - if (lastLambda < btScalar(1.)) - { - - result.m_fraction = lastLambda; - result.m_normal = n; - return true; - } + //is n normalized? + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if (n.dot(r)>=-result.m_allowedPenetration) + return false; + + result.m_fraction = lambda; + result.m_normal = n; + result.m_hitPoint = c; + return true; } return false; + + } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index 3905c45e6d6..a977c9e83f7 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 "../CollisionShapes/btCollisionMargin.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "../../LinearMath/btVector3.h" +#include "LinearMath/btVector3.h" #include "btConvexCast.h" class btConvexShape; class btMinkowskiSumShape; @@ -30,12 +30,12 @@ class btMinkowskiSumShape; class btGjkConvexCast : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; - btConvexShape* m_convexA; - btConvexShape* m_convexB; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver); + btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver); /// cast a convex against another convex object virtual bool calcTimeOfImpact( diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp index 8abdfdbb7e5..36cdeeaefdb 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp @@ -26,7 +26,7 @@ Nov.2006 #include "btGjkEpa.h" #include <string.h> //for memset -#include <LinearMath/btStackAlloc.h> +#include "LinearMath/btStackAlloc.h" #if defined(DEBUG) || defined (_DEBUG) #include <stdio.h> //for debug printf @@ -580,8 +580,8 @@ using namespace gjkepa_impl; // -bool btGjkEpaSolver::Collide(btConvexShape *shape0,const btTransform &wtrs0, - btConvexShape *shape1,const btTransform &wtrs1, +bool btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0, + const btConvexShape *shape1,const btTransform &wtrs1, btScalar radialmargin, btStackAlloc* stackAlloc, sResults& results) @@ -602,13 +602,13 @@ GJK gjk(stackAlloc, wtrs1.getBasis(),wtrs1.getOrigin(),shape1, radialmargin+EPA_accuracy); const Z collide(gjk.SearchOrigin()); -results.gjk_iterations = gjk.iterations+1; +results.gjk_iterations = static_cast<int>(gjk.iterations+1); if(collide) { /* Then EPA for penetration depth */ EPA epa(&gjk); const F pd(epa.EvaluatePD()); - results.epa_iterations = epa.iterations+1; + results.epa_iterations = static_cast<int>(epa.iterations+1); if(pd>0) { results.status = sResults::Penetrating; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h index 759b30bb17f..1c256f41939 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 "../CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" class btStackAlloc; @@ -35,7 +35,7 @@ struct sResults Separated, /* Shapes doesnt penetrate */ Penetrating, /* Shapes are penetrating */ GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ } status; btVector3 witnesses[2]; btVector3 normal; @@ -43,8 +43,8 @@ struct sResults int epa_iterations; int gjk_iterations; }; -static bool Collide(btConvexShape* shape0,const btTransform& wtrs0, - btConvexShape* shape1,const btTransform& wtrs1, +static bool Collide(const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, btScalar radialmargin, btStackAlloc* stackAlloc, sResults& results); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp new file mode 100644 index 00000000000..ccfc22ee673 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -0,0 +1,943 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "btGjkEpa2.h" + +#if defined(DEBUG) || defined (_DEBUG) +#include <stdio.h> //for debug printf +#ifdef __SPU__ +#include <spu_printf.h> +#define printf spu_printf +#endif //__SPU__ +#endif + +namespace gjkepa2_impl +{ + +// Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) + + +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff + { + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + btVector3 (btConvexShape::*Ls)(const btVector3&) const; + void EnableMargin(bool enable) + { + if(enable) + Ls=&btConvexShape::localGetSupportingVertex; + else + Ls=&btConvexShape::localGetSupportingVertexWithoutMargin; + } + inline btVector3 Support0(const btVector3& d) const + { + return(((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + } + inline btVector3 Support(const btVector3& d) const + { + return(Support0(d)-Support1(-d)); + } + btVector3 Support(const btVector3& d,U index) const + { + if(index) + return(Support1(d)); + else + return(Support0(d)); + } + }; + +typedef MinkowskiDiff tShape; + + +// GJK +struct GJK +{ +/* Types */ +struct sSV + { + btVector3 d,w; + }; +struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; +struct eStatus { enum _ { + Valid, + Inside, + Failed };}; +/* Fields */ +tShape m_shape; +btVector3 m_ray; +btScalar m_distance; +sSimplex m_simplices[2]; +sSV m_store[4]; +sSV* m_free[4]; +U m_nfree; +U m_current; +sSimplex* m_simplex; +eStatus::_ m_status; +/* Methods */ + GJK() + { + Initialize(); + } +void Initialize() + { + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } +eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + { + U iterations=0; + btScalar sqdist=0; + btScalar alpha=0; + btVector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rl<GJK_MIN_DISTANCE) + {/* Touching or inside */ + m_status=eStatus::Inside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs,-m_ray); + const btVector3& w=cs.c[cs.rank-1]->w; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) + { found=true;break; } + } + if(found) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + {/* Update lastw */ + lastw[clastw=(clastw+1)&3]=w; + } + /* Check for termination */ + const btScalar omega=dot(m_ray,w)/rl; + alpha=btMax(omega,alpha); + if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask=0; + switch(cs.rank) + { + case 2: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;i<ni;++i) + { + if(mask&(1<<i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; + } while(m_status==eStatus::Valid); + m_simplex=&m_simplices[m_current]; + switch(m_status) + { + case eStatus::Valid: m_distance=m_ray.length();break; + case eStatus::Inside: m_distance=0;break; + } + return(m_status); + } +bool EncloseOrigin() + { + switch(m_simplex->rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + const btVector3 p=cross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } +/* Internals */ +void getsupport(const btVector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } +void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } +void appendvertice(sSimplex& simplex,const btVector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } +static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + { + return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- + a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ + a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + btScalar* w,U& m) + { + const btVector3 d=b-a; + const btScalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const btScalar t(l>0?-dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=cross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2]; + U subm; + for(U i=0;i<3;++i) + { + if(dot(*vt[i],cross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if(mindist<0) + { + const btScalar d=dot(a,n); + const btScalar s=btSqrt(l); + const btVector3 p=n*(d/l); + mindist = p.length2(); + m = 7; + w[0] = (cross(dl[1],b-p)).length()/s; + w[1] = (cross(dl[2],c-p)).length()/s; + w[2] = 1-(w[0]+w[1]); + } + return(mindist); + } + return(-1); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c,&d}; + const btVector3 dl[]={a-d,b-d,c-d}; + const btScalar vl=det(dl[0],dl[1],dl[2]); + const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0; + if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) + { + btScalar mindist=-1; + btScalar subw[3]; + U subm; + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + if(s>0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>((subm&1?1<<i:0)+ + (subm&2?1<<j:0)+ + (subm&4?8:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if(mindist<0) + { + mindist = 0; + m = 15; + w[0] = det(c,b,d)/vl; + w[1] = det(a,c,d)/vl; + w[2] = det(b,a,d)/vl; + w[3] = 1-(w[0]+w[1]+w[2]); + } + return(mindist); + } + return(-1); + } +}; + +// EPA +struct EPA +{ +/* Types */ +typedef GJK::sSV sSV; +struct sFace + { + btVector3 n; + btScalar d; + btScalar p; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; +struct sList + { + sFace* root; + U count; + sList() : root(0),count(0) {} + }; +struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0),ff(0),nf(0) {} + }; +struct eStatus { enum _ { + Valid, + Touching, + Degenerated, + NonConvex, + InvalidHull, + OutOfFaces, + OutOfVertices, + AccuraryReached, + FallBack, + Failed };}; +/* Fields */ +eStatus::_ m_status; +GJK::sSimplex m_result; +btVector3 m_normal; +btScalar m_depth; +sSV m_sv_store[EPA_MAX_VERTICES]; +sFace m_fc_store[EPA_MAX_FACES]; +U m_nextsv; +sList m_hull; +sList m_stock; +/* Methods */ + EPA() + { + Initialize(); + } + + + static inline void bind(sFace* fa,U ea,sFace* fb,U eb) + { + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } +static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } +static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + +void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i<EPA_MAX_FACES;++i) + { + append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); + } + } +eStatus::_ Evaluate(GJK& gjk,const btVector3& guess) + { + GJK::sSimplex& simplex=*gjk.m_simplex; + if((simplex.rank>1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterations<EPA_MAX_ITERATIONS;++iterations) + { + if(m_nextsv<EPA_MAX_VERTICES) + { + sHorizon horizon; + sSV* w=&m_sv_store[m_nextsv++]; + bool valid=true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } +sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = cross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = btMin(btMin( + dot(a->w,cross(face->n,a->w-b->w)), + dot(b->w,cross(face->n,b->w-c->w))), + dot(c->w,cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); + } +sFace* findbest() + { + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + btScalar maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const btScalar sqd=f->d*f->d; + if((f->p>=maxp)&&(sqd<mind)) + { + minf=f; + mind=sqd; + maxp=f->p; + } + } + return(minf); + } +bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + +}; + +// +static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) +{ +/* Results */ +results.witnesses[0] = +results.witnesses[1] = btVector3(0,0,0); +results.status = btGjkEpaSolver2::sResults::Separated; +/* Shape */ +shape.m_shapes[0] = shape0; +shape.m_shapes[1] = shape1; +shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); +shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); +shape.EnableMargin(withmargins); +} + +} + +// +// Api +// + +using namespace gjkepa2_impl; + +// +int btGjkEpaSolver2::StackSizeRequirement() +{ +return(sizeof(GJK)+sizeof(EPA)); +} + +// +bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ +tShape shape; +Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); +if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;i<gjk.m_simplex->rank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); + } +} + +// +bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins) +{ +tShape shape; +Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); +switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + btVector3 w0=btVector3(0,0,0); + for(U i=0;i<epa.m_result.rank;++i) + { + w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + } +return(false); +} + +// +btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape0, + const btTransform& wtrs0, + sResults& results) +{ +tShape shape; +btSphereShape shape1(margin); +btTransform wtrs1(btQuaternion(0,0,0,1),position); +Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); +if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;i<gjk.m_simplex->rank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + const btVector3 delta= results.witnesses[1]- + results.witnesses[0]; + const btScalar margin= shape0->getMargin()+ + shape1.getMargin(); + const btScalar length= delta.length(); + results.normal = delta/length; + results.witnesses[0] += results.normal*margin; + return(length-margin); + } + else + { + if(gjk_status==GJK::eStatus::Inside) + { + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + { + const btVector3 delta= results.witnesses[0]- + results.witnesses[1]; + const btScalar length= delta.length(); + if (length >= SIMD_EPSILON) + results.normal = delta/length; + return(-length); + } + } + } +return(SIMD_INFINITY); +} + +// +bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ +if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) + return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + else + return(true); +} + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h new file mode 100644 index 00000000000..a55214203d3 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -0,0 +1,71 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct btGjkEpaSolver2 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar distance; + }; + +static int StackSizeRequirement(); + +static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results); + +static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins=true); + +static btScalar SignedDistance( const btVector3& position, + btScalar margin, + const btConvexShape* shape, + const btTransform& wtrs, + sResults& results); + +static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results); +}; + +#endif diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 87330493b60..c4f84ed4d75 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -18,9 +18,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, - btConvexShape* pConvexA, btConvexShape* pConvexB, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) @@ -32,10 +33,20 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim const btScalar radialmargin(btScalar(0.)); +//#define USE_ORIGINAL_GJK 1 +#ifdef USE_ORIGINAL_GJK btGjkEpaSolver::sResults results; if(btGjkEpaSolver::Collide( pConvexA,transformA, pConvexB,transformB, radialmargin,stackAlloc,results)) +#else + btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); + btGjkEpaSolver2::sResults results; + if(btGjkEpaSolver2::Penetration(pConvexA,transformA, + pConvexB,transformB, + guessVector,results)) + +#endif { // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 3916ba0776c..2dc069ce5cf 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -26,7 +26,7 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver public : bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - btConvexShape* pConvexA, btConvexShape* pConvexB, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index f1f3f7f7f6c..01fb1a4b068 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -35,7 +35,7 @@ int gNumGjkChecks = 0; -btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) +btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) :m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), @@ -47,7 +47,7 @@ m_catchDegeneracies(1) { } -void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) { btScalar distance=btScalar(0.); btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); @@ -144,6 +144,13 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& break; } + if(m_cachedSeparatingAxis.length2()<REL_ERROR2) + { + m_degenerateSimplex = 6; + checkSimplex = true; + break; + } + btScalar previousSquaredDistance = squaredDistance; squaredDistance = m_cachedSeparatingAxis.length2(); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index af0fe32f6c7..550fc4677e0 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -20,8 +20,8 @@ subject to the following restrictions: #define GJK_PAIR_DETECTOR_H #include "btDiscreteCollisionDetectorInterface.h" -#include "../../LinearMath/btPoint3.h" -#include "../CollisionShapes/btCollisionMargin.h" +#include "LinearMath/btPoint3.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" class btConvexShape; #include "btSimplexSolverInterface.h" @@ -35,8 +35,8 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface btVector3 m_cachedSeparatingAxis; btConvexPenetrationDepthSolver* m_penetrationDepthSolver; btSimplexSolverInterface* m_simplexSolver; - btConvexShape* m_minkowskiA; - btConvexShape* m_minkowskiB; + const btConvexShape* m_minkowskiA; + const btConvexShape* m_minkowskiB; bool m_ignoreMargin; @@ -49,10 +49,10 @@ public: int m_catchDegeneracies; - btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~btGjkPairDetector() {}; - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); void setMinkowskiA(btConvexShape* minkA) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index f6a893151da..e75fc1bee96 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" @@ -30,6 +30,8 @@ class btManifoldPoint public: btManifoldPoint() :m_userPersistentData(0), + m_appliedImpulse(0.f), + m_lateralFrictionInitialized(false), m_lifeTime(0) { } @@ -43,7 +45,11 @@ class btManifoldPoint m_distance1( distance ), m_combinedFriction(btScalar(0.)), m_combinedRestitution(btScalar(0.)), - m_userPersistentData(0), + m_userPersistentData(0), + m_appliedImpulse(0.f), + m_lateralFrictionInitialized(false), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), m_lifeTime(0) { @@ -63,11 +69,23 @@ class btManifoldPoint btScalar m_combinedFriction; btScalar m_combinedRestitution; + //BP mod, store contact triangles. + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; mutable void* m_userPersistentData; + btScalar m_appliedImpulse; - int m_lifeTime;//lifetime of the contactpoint in frames + bool m_lateralFrictionInitialized; + btScalar m_appliedImpulseLateral1; + btScalar m_appliedImpulseLateral2; + int m_lifeTime;//lifetime of the contactpoint in frames + btVector3 m_lateralFrictionDir1; + btVector3 m_lateralFrictionDir2; + btScalar getDistance() const { return m_distance1; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index c4bab3a134a..0e6fa2e6dfe 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -14,10 +14,10 @@ subject to the following restrictions: */ #include "btMinkowskiPenetrationDepthSolver.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" @@ -71,7 +71,7 @@ btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, - btConvexShape* convexA,btConvexShape* convexB, + const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btPoint3& pa, btPoint3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc @@ -112,8 +112,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //just take fixed number of orientation, and sample the penetration depth in that direction btScalar minProj = btScalar(1e30); - btVector3 minNorm; - btVector3 minVertex; + btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); btVector3 minA,minB; btVector3 seperatingAxisInA,seperatingAxisInB; btVector3 pInA,qInB,pWorld,qWorld,w; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index b348b21b52a..27b42c2b47e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -25,7 +25,7 @@ class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver public: virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - btConvexShape* convexA,btConvexShape* convexB, + const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btPoint3& pa, btPoint3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 08cb3ed334d..386885d2ac8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -20,6 +20,7 @@ subject to the following restrictions: btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; +ContactProcessedCallback gContactProcessedCallback = 0; @@ -27,20 +28,12 @@ btPersistentManifold::btPersistentManifold() :m_body0(0), m_body1(0), m_cachedPoints (0), -m_index1(0) +m_index1a(0) { } -void btPersistentManifold::clearManifold() -{ - int i; - for (i=0;i<m_cachedPoints;i++) - { - clearUserCache(m_pointCache[i]); - } - m_cachedPoints = 0; -} + #ifdef DEBUG_PERSISTENCY #include <stdio.h> @@ -169,7 +162,7 @@ int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const return nearestPoint; } -void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) +int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) { assert(validContactDistance(newPoint)); @@ -182,7 +175,7 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) #else insertIndex = 0; #endif - + clearUserCache(m_pointCache[insertIndex]); } else { @@ -190,7 +183,9 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) } - replaceContactPoint(newPoint,insertIndex); + btAssert(m_pointCache[insertIndex].m_userPersistentData==0); + m_pointCache[insertIndex] = newPoint; + return insertIndex; } btScalar btPersistentManifold::getContactBreakingThreshold() const @@ -198,10 +193,20 @@ btScalar btPersistentManifold::getContactBreakingThreshold() const return gContactBreakingThreshold; } + + void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) { int i; - +#ifdef DEBUG_PERSISTENCY + printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n", + trA.getOrigin().getX(), + trA.getOrigin().getY(), + trA.getOrigin().getZ(), + trB.getOrigin().getX(), + trB.getOrigin().getY(), + trB.getOrigin().getZ()); +#endif //DEBUG_PERSISTENCY /// first refresh worldspace positions and distance for (i=getNumContacts()-1;i>=0;i--) { @@ -232,6 +237,11 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() ) { removeContactPoint(i); + } else + { + //contact point processed callback + if (gContactProcessedCallback) + (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1); } } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index a5918b84db3..c122eb865e8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -17,9 +17,10 @@ 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" +#include "LinearMath/btAlignedAllocator.h" struct btCollisionResult; @@ -27,6 +28,7 @@ struct btCollisionResult; extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); +typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); extern ContactDestroyedCallback gContactDestroyedCallback; @@ -34,8 +36,13 @@ extern ContactDestroyedCallback gContactDestroyedCallback; #define MANIFOLD_CACHE_SIZE 4 -///btPersistentManifold maintains contact points, and reduces them to 4. -///It does contact filtering/contact reduction. +///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase. +///Those contact points are created by the collision narrow phase. +///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time. +///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large) +///reduces the cache to 4 points, when more then 4 points are added, using following rules: +///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points +///note that some pairs of objects might have more then one contact manifold. ATTRIBUTE_ALIGNED16( class) btPersistentManifold { @@ -55,20 +62,23 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold public: - int m_index1; + BT_DECLARE_ALIGNED_ALLOCATOR(); + + int m_index1a; btPersistentManifold(); - btPersistentManifold(void* body0,void* body1) + btPersistentManifold(void* body0,void* body1,int bla) : m_body0(body0),m_body1(body1),m_cachedPoints(0) { + (void)bla; } - inline void* getBody0() { return m_body0;} - inline void* getBody1() { return m_body1;} + SIMD_FORCE_INLINE void* getBody0() { return m_body0;} + SIMD_FORCE_INLINE void* getBody1() { return m_body1;} - inline const void* getBody0() const { return m_body0;} - inline const void* getBody1() const { return m_body1;} + SIMD_FORCE_INLINE const void* getBody0() const { return m_body0;} + SIMD_FORCE_INLINE const void* getBody1() const { return m_body1;} void setBodies(void* body0,void* body1) { @@ -82,15 +92,15 @@ public: void DebugPersistency(); #endif // - inline int getNumContacts() const { return m_cachedPoints;} + SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints;} - inline const btManifoldPoint& getContactPoint(int index) const + SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const { btAssert(index < m_cachedPoints); return m_pointCache[index]; } - inline btManifoldPoint& getContactPoint(int index) + SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index) { btAssert(index < m_cachedPoints); return m_pointCache[index]; @@ -101,7 +111,7 @@ public: int getCacheEntry(const btManifoldPoint& newPoint) const; - void AddManifoldPoint( const btManifoldPoint& newPoint); + int addManifoldPoint( const btManifoldPoint& newPoint); void removeContactPoint (int index) { @@ -114,6 +124,11 @@ public: m_pointCache[index] = m_pointCache[lastUsedIndex]; //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; + m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; + m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; + m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; + m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f; + m_pointCache[lastUsedIndex].m_lifeTime = 0; } btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); @@ -126,12 +141,20 @@ public: #define MAINTAIN_PERSISTENCY 1 #ifdef MAINTAIN_PERSISTENCY int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; + btAssert(lifeTime>=0); void* cache = m_pointCache[insertIndex].m_userPersistentData; m_pointCache[insertIndex] = newPoint; m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; + m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; + m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; + m_pointCache[insertIndex].m_lifeTime = lifeTime; #else clearUserCache(m_pointCache[insertIndex]); @@ -147,7 +170,16 @@ public: /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin void refreshContactPoints( const btTransform& trA,const btTransform& trB); - void clearManifold(); + + SIMD_FORCE_INLINE void clearManifold() + { + int i; + for (i=0;i<m_cachedPoints;i++) + { + clearUserCache(m_pointCache[i]); + } + m_cachedPoints = 0; + } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 31b91467777..a70a3ae56c4 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -13,7 +13,14 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - +//#include <stdio.h> + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "btRaycastCallback.h" btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to) @@ -29,8 +36,6 @@ btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { - - const btVector3 &vert0=triangle[0]; const btVector3 &vert1=triangle[1]; const btVector3 &vert2=triangle[2]; @@ -99,3 +104,60 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, } } } + + +btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) +{ + m_convexShape = convexShape; + m_convexShapeFrom = convexShapeFrom; + m_convexShapeTo = convexShapeTo; + m_triangleToWorld = triangleToWorld; + m_hitFraction = 1.0; + m_triangleCollisionMargin = triangleCollisionMargin; +} + +void +btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) +{ + btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); + triangleShape.setMargin(m_triangleCollisionMargin); + + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + +//#define USE_SUBSIMPLEX_CONVEX_CAST 1 +//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver); +#else + //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); + btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + + btConvexCast::CastResult castResult; + castResult.m_fraction = btScalar(1.); + if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < m_hitFraction) + { +/* btContinuousConvexCast's normal is already in world space */ +/* +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST +*/ + castResult.m_normal.normalize(); + + reportHit (castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction, + partId, + triangleIndex); + } + } + } +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index a0bbc9f8fe9..d2b4b80f8ba 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -16,15 +16,16 @@ subject to the following restrictions: #ifndef RAYCAST_TRI_CALLBACK_H #define RAYCAST_TRI_CALLBACK_H -#include "../CollisionShapes/btTriangleCallback.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "LinearMath/btTransform.h" struct btBroadphaseProxy; - +class btConvexShape; class btTriangleRaycastCallback: public btTriangleCallback { public: - //input + //input btVector3 m_from; btVector3 m_to; @@ -38,5 +39,22 @@ public: }; +class btTriangleConvexcastCallback : public btTriangleCallback +{ +public: + const btConvexShape* m_convexShape; + btTransform m_convexShapeFrom; + btTransform m_convexShapeTo; + btTransform m_triangleToWorld; + btScalar m_hitFraction; + btScalar m_triangleCollisionMargin; + + btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin); + + virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex); + + virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; +}; + #endif //RAYCAST_TRI_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index 58393b2eab9..cf65f46505b 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 687738b7fa9..4c709a8c3a9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -16,9 +16,11 @@ subject to the following restrictions: #include "btSubSimplexConvexCast.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" + #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" - +#include "btPointCollector.h" +#include "LinearMath/btTransformUtil.h" btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) :m_simplexSolver(simplexSolver), @@ -41,34 +43,24 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( CastResult& result) { - btMinkowskiSumShape combi(m_convexA,m_convexB); - btMinkowskiSumShape* convex = &combi; - - btTransform rayFromLocalA; - btTransform rayToLocalA; - - rayFromLocalA = fromA.inverse()* fromB; - rayToLocalA = toA.inverse()* toB; - - m_simplexSolver->reset(); - convex->setTransformB(btTransform(rayFromLocalA.getBasis())); - - //btScalar radius = btScalar(0.01); + btVector3 linVelA,linVelB; + linVelA = toA.getOrigin()-fromA.getOrigin(); + linVelB = toB.getOrigin()-fromB.getOrigin(); btScalar lambda = btScalar(0.); - //todo: need to verify this: - //because of minkowski difference, we need the inverse direction - - btVector3 s = -rayFromLocalA.getOrigin(); - btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin()); - btVector3 x = s; + + btTransform interpolatedTransA = fromA; + btTransform interpolatedTransB = fromB; + + ///take relative motion + btVector3 r = (linVelA-linVelB); btVector3 v; - btVector3 arbitraryPoint = convex->localGetSupportingVertex(r); - v = x - arbitraryPoint; - + btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis())); + btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis())); + v = supVertexA-supVertexB; int maxIter = MAX_ITERATIONS; btVector3 n; @@ -90,11 +82,17 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( while ( (dist2 > epsilon) && maxIter--) { - p = convex->localGetSupportingVertex( v); - w = x - p; + supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); + supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); + w = supVertexA-supVertexB; btScalar VdotW = v.dot(w); + if (lambda > btScalar(1.0)) + { + return false; + } + if ( VdotW > btScalar(0.)) { VdotR = v.dot(r); @@ -104,20 +102,25 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( else { lambda = lambda - VdotW / VdotR; - x = s + lambda * r; - m_simplexSolver->reset(); + //interpolate to next lambda + // x = s + lambda * r; + interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); + interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); + //m_simplexSolver->reset(); //check next line - w = x-p; + w = supVertexA-supVertexB; lastLambda = lambda; n = v; hasResult = true; } } - m_simplexSolver->addVertex( w, x , p); + m_simplexSolver->addVertex( w, supVertexA , supVertexB); if (m_simplexSolver->closest(v)) { dist2 = v.length2(); hasResult = true; + //todo: check this normal for validity + //n=v; //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); //printf("DIST2=%f\n",dist2); //printf("numverts = %i\n",m_simplexSolver->numVertices()); @@ -129,11 +132,26 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( //int numiter = MAX_ITERATIONS - maxIter; // printf("number of iterations: %d", numiter); - result.m_fraction = lambda; - result.m_normal = n; + + //don't report a time of impact when moving 'away' from the hitnormal + + result.m_fraction = lambda; + if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON)) + result.m_normal = n.normalized(); + else + result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if (result.m_normal.dot(r)>=-result.m_allowedPenetration) + return false; + + btVector3 hitA,hitB; + m_simplexSolver->compute_points(hitA,hitB); + result.m_hitPoint=hitB; return true; } + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 105b7eccefa..cf8a3ab5eb1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -26,7 +26,7 @@ subject to the following restrictions: #include "btVoronoiSimplexSolver.h" #include <assert.h> -#include <stdio.h> +//#include <stdio.h> #define VERTA 0 #define VERTB 1 |