diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision')
235 files changed, 26396 insertions, 25115 deletions
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index 77763305b1b..ec6fe9f4d8a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -2,7 +2,6 @@ //Bullet Continuous Collision Detection and Physics Library //Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - // // btAxisSweep3 // @@ -19,18 +18,15 @@ // 3. This notice may not be removed or altered from any source distribution. #include "btAxisSweep3.h" - -btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) -:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator) +btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) + : btAxisSweep3Internal<unsigned short int>(worldAabbMin, worldAabbMax, 0xfffe, 0xffff, maxHandles, pairCache, disableRaycastAccelerator) { // 1 handle is reserved as sentinel btAssert(maxHandles > 1 && maxHandles < 32767); - } - -bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) -:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator) +bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) + : btAxisSweep3Internal<unsigned int>(worldAabbMin, worldAabbMax, 0xfffffffe, 0x7fffffff, maxHandles, pairCache, disableRaycastAccelerator) { // 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 cd6e1a8929e..1e42f25f3bd 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -25,1005 +25,7 @@ #include "btBroadphaseProxy.h" #include "btOverlappingPairCallback.h" #include "btDbvtBroadphase.h" - -//#define DEBUG_BROADPHASE 1 -#define USE_OVERLAP_TEST_ON_REMOVES 1 - -/// 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: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - class Edge - { - 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 static_cast<BP_FP_INT_TYPE>(m_pos & 1);} - }; - -public: - 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_uniqueId; - btBroadphaseProxy* m_dbvtProxy;//for faster raycast - //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject - - 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 - - -protected: - btVector3 m_worldAabbMin; // overall system bounds - btVector3 m_worldAabbMax; // overall system bounds - - btVector3 m_quantize; // scaling factor for quantization - - BP_FP_INT_TYPE m_numHandles; // number of active 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; - - ///additional dynamic aabb structure, used to accelerate ray cast queries. - ///can be disabled using a optional argument in the constructor - btDbvtBroadphase* m_raycastAccelerator; - btOverlappingPairCache* m_nullPairCache; - - - // allocation/deallocation - BP_FP_INT_TYPE allocHandle(); - void freeHandle(BP_FP_INT_TYPE handle); - - - bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1); - -#ifdef DEBUG_BROADPHASE - void debugPrintAxis(int axis,bool checkCardinality=true); -#endif //DEBUG_BROADPHASE - - //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - - - - void sortMinDown(int axis, 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: - - btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false); - - virtual ~btAxisSweep3Internal(); - - BP_FP_INT_TYPE getNumHandles() const - { - return m_numHandles; - } - - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - - BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& 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 btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} - - virtual void resetPool(btDispatcher* dispatcher); - - void processAllOverlappingPairs(btOverlapCallback* callback); - - //Broadphase Interface - 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); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - - - void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; - ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result - void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - 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) - btAssert(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); - - if (m_raycastAccelerator) - { - btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0); - handle->m_dbvtProxy = rayProxy; - } - 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); - if (m_raycastAccelerator) - m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher); - 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); - handle->m_aabbMin = aabbMin; - handle->m_aabbMax = aabbMax; - updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); - if (m_raycastAccelerator) - m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher); - -} - -template <typename BP_FP_INT_TYPE> -void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) -{ - if (m_raycastAccelerator) - { - m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax); - } else - { - //choose axis? - BP_FP_INT_TYPE axis = 0; - //for each proxy - for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++) - { - if (m_pEdges[axis][i].IsMax()) - { - rayCallback.process(getHandle(m_pEdges[axis][i].m_handle)); - } - } - } -} - -template <typename BP_FP_INT_TYPE> -void btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) -{ - if (m_raycastAccelerator) - { - m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback); - } else - { - //choose axis? - BP_FP_INT_TYPE axis = 0; - //for each proxy - for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++) - { - if (m_pEdges[axis][i].IsMax()) - { - Handle* handle = getHandle(m_pEdges[axis][i].m_handle); - if (TestAabbAgainstAabb2(aabbMin,aabbMax,handle->m_aabbMin,handle->m_aabbMax)) - { - callback.process(handle); - } - } - } - } -} - - - -template <typename BP_FP_INT_TYPE> -void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - Handle* pHandle = static_cast<Handle*>(proxy); - aabbMin = pHandle->m_aabbMin; - aabbMax = pHandle->m_aabbMax; -} - - -template <typename BP_FP_INT_TYPE> -void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - Handle* pHandle = static_cast<Handle*>(proxy); - - unsigned short vecInMin[3]; - unsigned short vecInMax[3]; - - vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ; - vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ; - vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ; - vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ; - vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ; - vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ; - - aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ())); - aabbMin += m_worldAabbMin; - - aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ())); - aabbMax += m_worldAabbMin; -} - - - - -template <typename BP_FP_INT_TYPE> -btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) -:m_bpHandleMask(handleMask), -m_handleSentinel(handleSentinel), -m_pairCache(pairCache), -m_userPairCallback(0), -m_ownsPairCache(false), -m_invalidPair(0), -m_raycastAccelerator(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; - } - - if (!disableRaycastAccelerator) - { - m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache(); - m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache); - m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs - } - - //btAssert(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() -{ - if (m_raycastAccelerator) - { - m_nullPairCache->~btOverlappingPairCache(); - btAlignedFree(m_nullPairCache); - m_raycastAccelerator->~btDbvtBroadphase(); - btAlignedFree (m_raycastAccelerator); - } - - 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 btVector3& point, int isMax) const -{ -#ifdef OLD_CLAMPING_METHOD - ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax] - ///see http://code.google.com/p/bullet/issues/detail?id=87 - btVector3 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); -#else - btVector3 v = (point - m_worldAabbMin) * m_quantize; - out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax); - out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax); - out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax); -#endif //OLD_CLAMPING_METHOD -} - - -template <typename BP_FP_INT_TYPE> -BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle() -{ - btAssert(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) -{ - btAssert(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 btVector3& aabbMin,const btVector3& 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); - - -} - -template <typename BP_FP_INT_TYPE> -void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*/) -{ - if (m_numHandles == 0) - { - m_firstFreeHandle = 1; - { - for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++) - m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1)); - m_pHandles[m_maxHandles - 1].SetNextFree(0); - } - } -} - - -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) - { - ///important to use an AABB test that is consistent with the broadphase - 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 btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) -{ -// btAssert(bounds.IsFinite()); - //btAssert(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++; - } - -} - - - -//////////////////////////////////////////////////////////////////// - +#include "btAxisSweep3Internal.h" /// 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. @@ -1031,9 +33,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE ed class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int> { public: - - btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - + btAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); }; /// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune. @@ -1042,10 +42,7 @@ public: class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int> { public: - - bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - + bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); }; #endif - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h new file mode 100644 index 00000000000..2ee35528fd9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h @@ -0,0 +1,954 @@ +//Bullet Continuous Collision Detection and Physics Library +//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +// +// btAxisSweep3.h +// +// Copyright (c) 2006 Simon Hobbs +// +// 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_AXIS_SWEEP_3_INTERNAL_H +#define BT_AXIS_SWEEP_3_INTERNAL_H + +#include "LinearMath/btVector3.h" +#include "btOverlappingPairCache.h" +#include "btBroadphaseInterface.h" +#include "btBroadphaseProxy.h" +#include "btOverlappingPairCallback.h" +#include "btDbvtBroadphase.h" + +//#define DEBUG_BROADPHASE 1 +#define USE_OVERLAP_TEST_ON_REMOVES 1 + +/// 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: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + class Edge + { + 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 static_cast<BP_FP_INT_TYPE>(m_pos & 1); } + }; + +public: + 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_uniqueId; + btBroadphaseProxy* m_dbvtProxy; //for faster raycast + //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject + + 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 + +protected: + btVector3 m_worldAabbMin; // overall system bounds + btVector3 m_worldAabbMax; // overall system bounds + + btVector3 m_quantize; // scaling factor for quantization + + BP_FP_INT_TYPE m_numHandles; // number of active 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; + + ///additional dynamic aabb structure, used to accelerate ray cast queries. + ///can be disabled using a optional argument in the constructor + btDbvtBroadphase* m_raycastAccelerator; + btOverlappingPairCache* m_nullPairCache; + + // allocation/deallocation + BP_FP_INT_TYPE allocHandle(); + void freeHandle(BP_FP_INT_TYPE handle); + + bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1); + +#ifdef DEBUG_BROADPHASE + void debugPrintAxis(int axis, bool checkCardinality = true); +#endif //DEBUG_BROADPHASE + + //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); + //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); + + void sortMinDown(int axis, 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: + btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); + + virtual ~btAxisSweep3Internal(); + + BP_FP_INT_TYPE getNumHandles() const + { + return m_numHandles; + } + + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + + BP_FP_INT_TYPE addHandle(const btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher); + void removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher); + void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher); + SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const { return m_pHandles + index; } + + virtual void resetPool(btDispatcher* dispatcher); + + void processAllOverlappingPairs(btOverlapCallback* callback); + + //Broadphase Interface + virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher); + virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher); + virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const; + + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + + void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; + ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result + void unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const; + + 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) + btAssert(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, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) +{ + (void)shapeType; + BP_FP_INT_TYPE handleId = addHandle(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher); + + Handle* handle = getHandle(handleId); + + if (m_raycastAccelerator) + { + btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher); + handle->m_dbvtProxy = rayProxy; + } + 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); + if (m_raycastAccelerator) + m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy, dispatcher); + 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); + handle->m_aabbMin = aabbMin; + handle->m_aabbMax = aabbMax; + updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax, dispatcher); + if (m_raycastAccelerator) + m_raycastAccelerator->setAabb(handle->m_dbvtProxy, aabbMin, aabbMax, dispatcher); +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax) +{ + if (m_raycastAccelerator) + { + m_raycastAccelerator->rayTest(rayFrom, rayTo, rayCallback, aabbMin, aabbMax); + } + else + { + //choose axis? + BP_FP_INT_TYPE axis = 0; + //for each proxy + for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++) + { + if (m_pEdges[axis][i].IsMax()) + { + rayCallback.process(getHandle(m_pEdges[axis][i].m_handle)); + } + } + } +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +{ + if (m_raycastAccelerator) + { + m_raycastAccelerator->aabbTest(aabbMin, aabbMax, callback); + } + else + { + //choose axis? + BP_FP_INT_TYPE axis = 0; + //for each proxy + for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++) + { + if (m_pEdges[axis][i].IsMax()) + { + Handle* handle = getHandle(m_pEdges[axis][i].m_handle); + if (TestAabbAgainstAabb2(aabbMin, aabbMax, handle->m_aabbMin, handle->m_aabbMax)) + { + callback.process(handle); + } + } + } + } +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const +{ + Handle* pHandle = static_cast<Handle*>(proxy); + aabbMin = pHandle->m_aabbMin; + aabbMax = pHandle->m_aabbMax; +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const +{ + Handle* pHandle = static_cast<Handle*>(proxy); + + unsigned short vecInMin[3]; + unsigned short vecInMax[3]; + + vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos; + vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos + 1; + vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos; + vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos + 1; + vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos; + vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos + 1; + + aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()), (btScalar)(vecInMin[1]) / (m_quantize.getY()), (btScalar)(vecInMin[2]) / (m_quantize.getZ())); + aabbMin += m_worldAabbMin; + + aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()), (btScalar)(vecInMax[1]) / (m_quantize.getY()), (btScalar)(vecInMax[2]) / (m_quantize.getZ())); + aabbMax += m_worldAabbMin; +} + +template <typename BP_FP_INT_TYPE> +btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) + : m_bpHandleMask(handleMask), + m_handleSentinel(handleSentinel), + m_pairCache(pairCache), + m_userPairCallback(0), + m_ownsPairCache(false), + m_invalidPair(0), + m_raycastAccelerator(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; + } + + if (!disableRaycastAccelerator) + { + m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache), 16)) btNullPairCache(); + m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase), 16)) btDbvtBroadphase(m_nullPairCache); //m_pairCache); + m_raycastAccelerator->m_deferedcollide = true; //don't add/remove pairs + } + + //btAssert(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() +{ + if (m_raycastAccelerator) + { + m_nullPairCache->~btOverlappingPairCache(); + btAlignedFree(m_nullPairCache); + m_raycastAccelerator->~btDbvtBroadphase(); + btAlignedFree(m_raycastAccelerator); + } + + 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 btVector3& point, int isMax) const +{ +#ifdef OLD_CLAMPING_METHOD + ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax] + ///see http://code.google.com/p/bullet/issues/detail?id=87 + btVector3 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); +#else + btVector3 v = (point - m_worldAabbMin) * m_quantize; + out[0] = (v[0] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[0] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0] & m_bpHandleMask) | isMax); + out[1] = (v[1] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[1] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1] & m_bpHandleMask) | isMax); + out[2] = (v[2] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[2] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2] & m_bpHandleMask) | isMax); +#endif //OLD_CLAMPING_METHOD +} + +template <typename BP_FP_INT_TYPE> +BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle() +{ + btAssert(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) +{ + btAssert(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 btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) +{ + // 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; + + // 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); +} + +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*/) +{ + if (m_numHandles == 0) + { + m_firstFreeHandle = 1; + { + for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++) + m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1)); + m_pHandles[m_maxHandles - 1].SetNextFree(0); + } + } +} + +//#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) + { + ///important to use an AABB test that is consistent with the broadphase + 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++; + } + } + +///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 btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher) +{ + // btAssert(bounds.IsFinite()); + //btAssert(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++; + } +} + +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index f1bf00594d3..b097eca5f5b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -13,10 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BT_BROADPHASE_INTERFACE_H -#define BT_BROADPHASE_INTERFACE_H - - +#ifndef BT_BROADPHASE_INTERFACE_H +#define BT_BROADPHASE_INTERFACE_H struct btDispatcherInfo; class btDispatcher; @@ -24,23 +22,23 @@ class btDispatcher; class btOverlappingPairCache; - - -struct btBroadphaseAabbCallback +struct btBroadphaseAabbCallback { virtual ~btBroadphaseAabbCallback() {} - virtual bool process(const btBroadphaseProxy* proxy) = 0; + virtual bool process(const btBroadphaseProxy* proxy) = 0; }; - -struct btBroadphaseRayCallback : public btBroadphaseAabbCallback +struct btBroadphaseRayCallback : public btBroadphaseAabbCallback { ///added some cached data to accelerate ray-AABB tests - btVector3 m_rayDirectionInverse; - unsigned int m_signs[3]; - btScalar m_lambda_max; + btVector3 m_rayDirectionInverse; + unsigned int m_signs[3]; + btScalar m_lambda_max; virtual ~btBroadphaseRayCallback() {} + +protected: + btBroadphaseRayCallback() {} }; #include "LinearMath/btVector3.h" @@ -53,30 +51,29 @@ class btBroadphaseInterface public: virtual ~btBroadphaseInterface() {} - 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; - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0; + virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) = 0; + virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0; + virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher) = 0; + virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0; + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)) = 0; - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0; + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0; ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb - virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; + virtual void calculateOverlappingPairs(btDispatcher* dispatcher) = 0; - virtual btOverlappingPairCache* getOverlappingPairCache()=0; - virtual const btOverlappingPairCache* getOverlappingPairCache() const =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 getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const = 0; ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; }; - - virtual void printStats() = 0; + virtual void resetPool(btDispatcher* dispatcher) { (void)dispatcher; }; + virtual void printStats() = 0; }; -#endif //BT_BROADPHASE_INTERFACE_H +#endif //BT_BROADPHASE_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp index f4d7341f8dd..7ee065aac32 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp @@ -15,3 +15,4 @@ subject to the following restrictions: #include "btBroadphaseProxy.h" +BT_NOT_EMPTY_FILE // fix warning LNK4221: This object file does not define any previously undefined public symbols, so it will not be used by any link operation that consumes this library diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index bb58b828936..bc0742ad623 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -16,11 +16,10 @@ subject to the following restrictions: #ifndef BT_BROADPHASE_PROXY_H #define BT_BROADPHASE_PROXY_H -#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE +#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE #include "LinearMath/btVector3.h" #include "LinearMath/btAlignedAllocator.h" - /// btDispatcher uses these types /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave /// to facilitate type checking @@ -35,8 +34,8 @@ enum BroadphaseNativeTypes CONVEX_HULL_SHAPE_PROXYTYPE, CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE, CUSTOM_POLYHEDRAL_SHAPE_TYPE, -//implicit convex shapes -IMPLICIT_CONVEX_SHAPES_START_HERE, + //implicit convex shapes + IMPLICIT_CONVEX_SHAPES_START_HERE, SPHERE_SHAPE_PROXYTYPE, MULTI_SPHERE_SHAPE_PROXYTYPE, CAPSULE_SHAPE_PROXYTYPE, @@ -49,8 +48,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE, BOX_2D_SHAPE_PROXYTYPE, CONVEX_2D_SHAPE_PROXYTYPE, CUSTOM_CONVEX_SHAPE_TYPE, -//concave shapes -CONCAVE_SHAPES_START_HERE, + //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, @@ -58,15 +57,16 @@ CONCAVE_SHAPES_START_HERE, FAST_CONCAVE_MESH_PROXYTYPE, //terrain TERRAIN_SHAPE_PROXYTYPE, -///Used for GIMPACT Trimesh integration + ///Used for GIMPACT Trimesh integration GIMPACT_SHAPE_PROXYTYPE, -///Multimaterial mesh - MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE, - + ///Multimaterial mesh + MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE, + EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, CUSTOM_CONCAVE_SHAPE_TYPE, -CONCAVE_SHAPES_END_HERE, + SDF_SHAPE_PROXYTYPE = CUSTOM_CONCAVE_SHAPE_TYPE, + CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, @@ -76,38 +76,37 @@ CONCAVE_SHAPES_END_HERE, INVALID_SHAPE_PROXYTYPE, MAX_BROADPHASE_COLLISION_TYPES - -}; +}; -///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. +///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 +ATTRIBUTE_ALIGNED16(struct) +btBroadphaseProxy { + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_DECLARE_ALIGNED_ALLOCATOR(); - ///optional filtering to cull potential collisions enum CollisionFilterGroups { - DefaultFilter = 1, - StaticFilter = 2, - KinematicFilter = 4, - DebrisFilter = 8, - SensorTrigger = 16, - CharacterFilter = 32, - AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + DebrisFilter = 8, + SensorTrigger = 16, + CharacterFilter = 32, + 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. + void* m_clientObject; + int m_collisionFilterGroup; + int m_collisionFilterMask; - btVector3 m_aabbMin; - btVector3 m_aabbMax; + int m_uniqueId; //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. + + btVector3 m_aabbMin; + btVector3 m_aabbMax; SIMD_FORCE_INLINE int getUid() const { @@ -115,48 +114,45 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); } //used for memory pools - btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0) + btBroadphaseProxy() : m_clientObject(0) { } - btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) - :m_clientObject(userPtr), - m_collisionFilterGroup(collisionFilterGroup), - m_collisionFilterMask(collisionFilterMask), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax) + btBroadphaseProxy(const btVector3& aabbMin, const btVector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) + : m_clientObject(userPtr), + m_collisionFilterGroup(collisionFilterGroup), + m_collisionFilterMask(collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) { - m_multiSapParentProxy = multiSapParentProxy; } - - static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType) { - return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE); + return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE); } - static SIMD_FORCE_INLINE bool isConvex(int proxyType) + static SIMD_FORCE_INLINE bool isConvex(int proxyType) { return (proxyType < CONCAVE_SHAPES_START_HERE); } - static SIMD_FORCE_INLINE bool isNonMoving(int proxyType) + static SIMD_FORCE_INLINE bool isNonMoving(int proxyType) { - return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE)); + return (isConcave(proxyType) && !(proxyType == GIMPACT_SHAPE_PROXYTYPE)); } - static SIMD_FORCE_INLINE bool isConcave(int proxyType) + static SIMD_FORCE_INLINE bool isConcave(int proxyType) { return ((proxyType > CONCAVE_SHAPES_START_HERE) && - (proxyType < CONCAVE_SHAPES_END_HERE)); + (proxyType < CONCAVE_SHAPES_END_HERE)); } - static SIMD_FORCE_INLINE bool isCompound(int proxyType) + static SIMD_FORCE_INLINE bool isCompound(int proxyType) { return (proxyType == COMPOUND_SHAPE_PROXYTYPE); } - static SIMD_FORCE_INLINE bool isSoftBody(int proxyType) + static SIMD_FORCE_INLINE bool isSoftBody(int proxyType) { return (proxyType == SOFTBODY_SHAPE_PROXYTYPE); } @@ -168,67 +164,55 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); static SIMD_FORCE_INLINE bool isConvex2d(int proxyType) { - return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE); + return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE); } - - -} -; +}; class btCollisionAlgorithm; struct btBroadphaseProxy; - - ///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 +ATTRIBUTE_ALIGNED16(struct) +btBroadphasePair { - btBroadphasePair () - : - m_pProxy0(0), - m_pProxy1(0), - m_algorithm(0), - m_internalInfo1(0) + btBroadphasePair() + : m_pProxy0(0), + m_pProxy1(0), + m_algorithm(0), + m_internalInfo1(0) { } -BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); - btBroadphasePair(const btBroadphasePair& other) - : m_pProxy0(other.m_pProxy0), - m_pProxy1(other.m_pProxy1), - m_algorithm(other.m_algorithm), - m_internalInfo1(other.m_internalInfo1) - { - } - btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) + btBroadphasePair(btBroadphaseProxy & proxy0, btBroadphaseProxy & proxy1) { - //keep them sorted, so the std::set operations work if (proxy0.m_uniqueId < proxy1.m_uniqueId) - { - m_pProxy0 = &proxy0; - m_pProxy1 = &proxy1; - } - else - { - m_pProxy0 = &proxy1; - m_pProxy1 = &proxy0; - } + { + m_pProxy0 = &proxy0; + m_pProxy1 = &proxy1; + } + else + { + m_pProxy0 = &proxy1; + m_pProxy1 = &proxy0; + } m_algorithm = 0; m_internalInfo1 = 0; - } - + btBroadphaseProxy* m_pProxy0; btBroadphaseProxy* m_pProxy1; - - mutable btCollisionAlgorithm* m_algorithm; - union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version. + mutable btCollisionAlgorithm* m_algorithm; + union { + void* m_internalInfo1; + int m_internalTmpValue; + }; //don't use this data, it will be removed in future version. }; /* @@ -240,31 +224,25 @@ SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePa } */ - - class btBroadphasePairSortPredicate { - public: - - bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const - { - const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1; - const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1; - const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1; - const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1; - - return uidA0 > uidB0 || - (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) || - (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); - } +public: + bool operator()(const btBroadphasePair& a, const btBroadphasePair& b) const + { + const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1; + const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1; + const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1; + const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1; + + return uidA0 > uidB0 || + (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) || + (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); + } }; - -SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) +SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) { - return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1); + return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1); } - -#endif //BT_BROADPHASE_PROXY_H - +#endif //BT_BROADPHASE_PROXY_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp index c95d1be0f2c..6e36d3bd730 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp @@ -20,4 +20,3 @@ btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructio { m_dispatcher = ci.m_dispatcher1; } - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index 405656236bd..b00c0b1b4bb 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -25,57 +25,51 @@ class btManifoldResult; class btCollisionObject; struct btCollisionObjectWrapper; struct btDispatcherInfo; -class btPersistentManifold; +class btPersistentManifold; -typedef btAlignedObjectArray<btPersistentManifold*> btManifoldArray; +typedef btAlignedObjectArray<btPersistentManifold*> btManifoldArray; struct btCollisionAlgorithmConstructionInfo { btCollisionAlgorithmConstructionInfo() - :m_dispatcher1(0), - m_manifold(0) + : m_dispatcher1(0), + m_manifold(0) { } - btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) - :m_dispatcher1(dispatcher) + btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher, int temp) + : m_dispatcher1(dispatcher) { (void)temp; } - btDispatcher* m_dispatcher1; - btPersistentManifold* m_manifold; - -// int getDispatcherId(); + btDispatcher* m_dispatcher1; + btPersistentManifold* m_manifold; + // int getDispatcherId(); }; - ///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher. ///It is persistent over frames class btCollisionAlgorithm { - protected: - - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher; protected: -// int getDispatcherId(); - -public: + // int getDispatcherId(); - btCollisionAlgorithm() {}; +public: + btCollisionAlgorithm(){}; btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - virtual ~btCollisionAlgorithm() {}; + virtual ~btCollisionAlgorithm(){}; - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0; - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0; - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0; + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0; }; - -#endif //BT_COLLISION_ALGORITHM_H +#endif //BT_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 2ca20cdd8b8..166cb04c0b0 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -17,210 +17,226 @@ subject to the following restrictions: #include "btDbvt.h" // -typedef btAlignedObjectArray<btDbvtNode*> tNodeArray; -typedef btAlignedObjectArray<const btDbvtNode*> tConstNodeArray; +typedef btAlignedObjectArray<btDbvtNode*> tNodeArray; +typedef btAlignedObjectArray<const btDbvtNode*> tConstNodeArray; // struct btDbvtNodeEnumerator : btDbvt::ICollide { - tConstNodeArray nodes; + tConstNodeArray nodes; void Process(const btDbvtNode* n) { nodes.push_back(n); } }; // -static DBVT_INLINE int indexof(const btDbvtNode* node) +static DBVT_INLINE int indexof(const btDbvtNode* node) { - return(node->parent->childs[1]==node); + return (node->parent->childs[1] == node); } // -static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, +static DBVT_INLINE btDbvtVolume merge(const btDbvtVolume& a, const btDbvtVolume& b) { -#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) - ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]); - btDbvtVolume* ptr = (btDbvtVolume*) locals; - btDbvtVolume& res=*ptr; +#ifdef BT_USE_SSE + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); + btDbvtVolume* ptr = (btDbvtVolume*)locals; + btDbvtVolume& res = *ptr; #else - btDbvtVolume res; + btDbvtVolume res; #endif - Merge(a,b,res); - return(res); + Merge(a, b, res); + return (res); } // volume+edge lengths -static DBVT_INLINE btScalar size(const btDbvtVolume& a) +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()); + 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) +static void getmaxdepth(const btDbvtNode* node, int depth, int& maxdepth) { - if(node->isinternal()) + if (node->isinternal()) { - getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[1],depth+1,maxdepth); - } else maxdepth=btMax(maxdepth,depth); + getmaxdepth(node->childs[0], depth + 1, maxdepth); + getmaxdepth(node->childs[1], depth + 1, maxdepth); + } + else + maxdepth = btMax(maxdepth, depth); } // -static DBVT_INLINE void deletenode( btDbvt* pdbvt, - btDbvtNode* node) +static DBVT_INLINE void deletenode(btDbvt* pdbvt, + btDbvtNode* node) { btAlignedFree(pdbvt->m_free); - pdbvt->m_free=node; + pdbvt->m_free = node; } // -static void recursedeletenode( btDbvt* pdbvt, - btDbvtNode* node) +static void recursedeletenode(btDbvt* pdbvt, + btDbvtNode* node) { - if(!node->isleaf()) + if (node == 0) return; + if (!node->isleaf()) { - recursedeletenode(pdbvt,node->childs[0]); - recursedeletenode(pdbvt,node->childs[1]); + recursedeletenode(pdbvt, node->childs[0]); + recursedeletenode(pdbvt, node->childs[1]); } - if(node==pdbvt->m_root) pdbvt->m_root=0; - deletenode(pdbvt,node); + if (node == pdbvt->m_root) pdbvt->m_root = 0; + deletenode(pdbvt, node); } // -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - void* data) +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; } + 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); + { + 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) +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); + 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) +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); + btDbvtNode* node = createnode(pdbvt, parent, data); + Merge(volume0, volume1, node->volume); + return (node); } // -static void insertleaf( btDbvt* pdbvt, - btDbvtNode* root, - btDbvtNode* leaf) +static void insertleaf(btDbvt* pdbvt, + btDbvtNode* root, + btDbvtNode* leaf) { - if(!pdbvt->m_root) + if (!pdbvt->m_root) { - pdbvt->m_root = leaf; - leaf->parent = 0; + pdbvt->m_root = leaf; + leaf->parent = 0; } else { - if(!root->isleaf()) + if (!root->isleaf()) { - do { - root=root->childs[Select( leaf->volume, - root->childs[0]->volume, - root->childs[1]->volume)]; - } while(!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) + 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); + 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)); + 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; + 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) +static btDbvtNode* removeleaf(btDbvt* pdbvt, + btDbvtNode* leaf) { - if(leaf==pdbvt->m_root) + if (leaf == pdbvt->m_root) { - pdbvt->m_root=0; - return(0); + pdbvt->m_root = 0; + return (0); } else { - btDbvtNode* parent=leaf->parent; - btDbvtNode* prev=parent->parent; - btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; - if(prev) + 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) + 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)) + 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; + prev = prev->parent; + } + else + break; } - return(prev?prev:pdbvt->m_root); + return (prev ? prev : pdbvt->m_root); } else - { - pdbvt->m_root=sibling; - sibling->parent=0; - deletenode(pdbvt,parent); - return(pdbvt->m_root); - } + { + 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) +static void fetchleaves(btDbvt* pdbvt, + btDbvtNode* root, + tNodeArray& leaves, + int depth = -1) { - if(root->isinternal()&&depth) + if (root->isinternal() && depth) { - fetchleaves(pdbvt,root->childs[0],leaves,depth-1); - fetchleaves(pdbvt,root->childs[1],leaves,depth-1); - deletenode(pdbvt,root); + fetchleaves(pdbvt, root->childs[0], leaves, depth - 1); + fetchleaves(pdbvt, root->childs[1], leaves, depth - 1); + deletenode(pdbvt, root); } else { @@ -229,170 +245,204 @@ static void fetchleaves(btDbvt* pdbvt, } // -static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, - const btVector3& org, - const btVector3& axis) +static bool leftOfAxis(const btDbvtNode* node, + const btVector3& org, + const btVector3& axis) { - left.resize(0); - right.resize(0); - for(int i=0,ni=leaves.size();i<ni;++i) + return btDot(axis, node->volume.Center() - org) <= 0; +} + +// Partitions leaves such that leaves[0, n) are on the +// left of axis, and leaves[n, count) are on the right +// of axis. returns N. +static int split(btDbvtNode** leaves, + int count, + const btVector3& org, + const btVector3& axis) +{ + int begin = 0; + int end = count; + for (;;) { - if(btDot(axis,leaves[i]->volume.Center()-org)<0) - left.push_back(leaves[i]); - else - right.push_back(leaves[i]); + while (begin != end && leftOfAxis(leaves[begin], org, axis)) + { + ++begin; + } + + if (begin == end) + { + break; + } + + while (begin != end && !leftOfAxis(leaves[end - 1], org, axis)) + { + --end; + } + + if (begin == end) + { + break; + } + + // swap out of place nodes + --end; + btDbvtNode* temp = leaves[begin]; + leaves[begin] = leaves[end]; + leaves[end] = temp; + ++begin; } + + return begin; } // -static btDbvtVolume bounds( const tNodeArray& leaves) +static btDbvtVolume bounds(btDbvtNode** leaves, + int count) { -#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE - ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); - btDbvtVolume* ptr = (btDbvtVolume*) locals; - btDbvtVolume& volume=*ptr; - volume=leaves[0]->volume; +#ifdef BT_USE_SSE + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); + btDbvtVolume* ptr = (btDbvtVolume*)locals; + btDbvtVolume& volume = *ptr; + volume = leaves[0]->volume; #else - btDbvtVolume volume=leaves[0]->volume; + btDbvtVolume volume = leaves[0]->volume; #endif - for(int i=1,ni=leaves.size();i<ni;++i) + for (int i = 1, ni = count; i < ni; ++i) { - Merge(volume,leaves[i]->volume,volume); + Merge(volume, leaves[i]->volume, volume); } - return(volume); + return (volume); } // -static void bottomup( btDbvt* pdbvt, - tNodeArray& leaves) +static void bottomup(btDbvt* pdbvt, + btDbvtNode** leaves, + int count) { - while(leaves.size()>1) + while (count > 1) { - btScalar minsize=SIMD_INFINITY; - int minidx[2]={-1,-1}; - for(int i=0;i<leaves.size();++i) + btScalar minsize = SIMD_INFINITY; + int minidx[2] = {-1, -1}; + for (int i = 0; i < count; ++i) { - for(int j=i+1;j<leaves.size();++j) + for (int j = i + 1; j < count; ++j) { - const btScalar sz=size(merge(leaves[i]->volume,leaves[j]->volume)); - if(sz<minsize) + const btScalar sz = size(merge(leaves[i]->volume, leaves[j]->volume)); + if (sz < minsize) { - minsize = sz; - minidx[0] = i; - minidx[1] = j; + 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(); + 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[minidx[1]] = leaves[count - 1]; + --count; } } // -static btDbvtNode* topdown(btDbvt* pdbvt, - tNodeArray& leaves, - int bu_treshold) +static btDbvtNode* topdown(btDbvt* pdbvt, + btDbvtNode** leaves, + int count, + int bu_treshold) { - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - if(leaves.size()>1) + static const btVector3 axis[] = {btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1)}; + btAssert(bu_treshold > 2); + if (count > 1) { - if(leaves.size()>bu_treshold) + if (count > 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}}; + const btDbvtVolume vol = bounds(leaves, count); + const btVector3 org = vol.Center(); + int partition; + int bestaxis = -1; + int bestmidp = count; + int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; int i; - for( i=0;i<leaves.size();++i) + for (i = 0; i < count; ++i) { - const btVector3 x=leaves[i]->volume.Center()-org; - for(int j=0;j<3;++j) + const btVector3 x = leaves[i]->volume.Center() - org; + for (int j = 0; j < 3; ++j) { - ++splitcount[j][btDot(x,axis[j])>0?1:0]; + ++splitcount[j][btDot(x, axis[j]) > 0 ? 1 : 0]; } } - for( i=0;i<3;++i) + for (i = 0; i < 3; ++i) { - if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { - const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); - if(midp<bestmidp) + const int midp = (int)btFabs(btScalar(splitcount[i][0] - splitcount[i][1])); + if (midp < bestmidp) { - bestaxis=i; - bestmidp=midp; + bestaxis = i; + bestmidp = midp; } } } - if(bestaxis>=0) + if (bestaxis >= 0) { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leaves,sets[0],sets[1],org,axis[bestaxis]); + partition = split(leaves, count, org, axis[bestaxis]); + btAssert(partition != 0 && partition != count); } 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]); - } + partition = count / 2 + 1; } - 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); + btDbvtNode* node = createnode(pdbvt, 0, vol, 0); + node->childs[0] = topdown(pdbvt, &leaves[0], partition, bu_treshold); + node->childs[1] = topdown(pdbvt, &leaves[partition], count - partition, bu_treshold); + node->childs[0]->parent = node; + node->childs[1]->parent = node; + return (node); } else { - bottomup(pdbvt,leaves); - return(leaves[0]); + bottomup(pdbvt, leaves, count); + return (leaves[0]); } } - return(leaves[0]); + return (leaves[0]); } // -static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) +static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n, btDbvtNode*& r) { - btDbvtNode* p=n->parent; + btDbvtNode* p = n->parent; btAssert(n->isinternal()); - if(p>n) + 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); + 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); + return (n); } #if 0 @@ -410,11 +460,11 @@ static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) // btDbvt::btDbvt() { - m_root = 0; - m_free = 0; - m_lkhd = -1; - m_leaves = 0; - m_opath = 0; + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; } // @@ -424,228 +474,233 @@ btDbvt::~btDbvt() } // -void btDbvt::clear() +void btDbvt::clear() { - if(m_root) - recursedeletenode(this,m_root); + if (m_root) + recursedeletenode(this, m_root); btAlignedFree(m_free); - m_free=0; - m_lkhd = -1; + m_free = 0; + m_lkhd = -1; m_stkStack.clear(); - m_opath = 0; - + m_opath = 0; } // -void btDbvt::optimizeBottomUp() +void btDbvt::optimizeBottomUp() { - if(m_root) + if (m_root) { tNodeArray leaves; leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - bottomup(this,leaves); - m_root=leaves[0]; + fetchleaves(this, m_root, leaves); + bottomup(this, &leaves[0], leaves.size()); + m_root = leaves[0]; } } // -void btDbvt::optimizeTopDown(int bu_treshold) +void btDbvt::optimizeTopDown(int bu_treshold) { - if(m_root) + if (m_root) { - tNodeArray leaves; + tNodeArray leaves; leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); + fetchleaves(this, m_root, leaves); + m_root = topdown(this, &leaves[0], leaves.size(), bu_treshold); } } // -void btDbvt::optimizeIncremental(int passes) +void btDbvt::optimizeIncremental(int passes) { - if(passes<0) passes=m_leaves; - if(m_root&&(passes>0)) + if (passes < 0) passes = m_leaves; + if (m_root && (passes > 0)) { - do { - btDbvtNode* node=m_root; - unsigned bit=0; - while(node->isinternal()) + 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); + node = sort(node, m_root)->childs[(m_opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned) * 8 - 1); } update(node); ++m_opath; - } while(--passes); + } while (--passes); } } // -btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) +btDbvtNode* btDbvt::insert(const btDbvtVolume& volume, void* data) { - btDbvtNode* leaf=createnode(this,0,volume,data); - insertleaf(this,m_root,leaf); + btDbvtNode* leaf = createnode(this, 0, volume, data); + insertleaf(this, m_root, leaf); ++m_leaves; - return(leaf); + return (leaf); } // -void btDbvt::update(btDbvtNode* leaf,int lookahead) +void btDbvt::update(btDbvtNode* leaf, int lookahead) { - btDbvtNode* root=removeleaf(this,leaf); - if(root) + btDbvtNode* root = removeleaf(this, leaf); + if (root) { - if(lookahead>=0) + if (lookahead >= 0) { - for(int i=0;(i<lookahead)&&root->parent;++i) + for (int i = 0; (i < lookahead) && root->parent; ++i) { - root=root->parent; + root = root->parent; } - } else root=m_root; + } + else + root = m_root; } - insertleaf(this,root,leaf); + insertleaf(this, root, leaf); } // -void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume) +void btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume) { - btDbvtNode* root=removeleaf(this,leaf); - if(root) + btDbvtNode* root = removeleaf(this, leaf); + if (root) { - if(m_lkhd>=0) + if (m_lkhd >= 0) { - for(int i=0;(i<m_lkhd)&&root->parent;++i) + for (int i = 0; (i < m_lkhd) && root->parent; ++i) { - root=root->parent; + root = root->parent; } - } else root=m_root; + } + else + root = m_root; } - leaf->volume=volume; - insertleaf(this,root,leaf); + leaf->volume = volume; + insertleaf(this, root, leaf); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin) +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)); + if (leaf->volume.Contain(volume)) return (false); + volume.Expand(btVector3(margin, margin, margin)); volume.SignedExpand(velocity); - update(leaf,volume); - return(true); + update(leaf, volume); + return (true); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity) +bool btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity) { - if(leaf->volume.Contain(volume)) return(false); + if (leaf->volume.Contain(volume)) return (false); volume.SignedExpand(velocity); - update(leaf,volume); - return(true); + update(leaf, volume); + return (true); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin) +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); + if (leaf->volume.Contain(volume)) return (false); + volume.Expand(btVector3(margin, margin, margin)); + update(leaf, volume); + return (true); } // -void btDbvt::remove(btDbvtNode* leaf) +void btDbvt::remove(btDbvtNode* leaf) { - removeleaf(this,leaf); - deletenode(this,leaf); + removeleaf(this, leaf); + deletenode(this, leaf); --m_leaves; } // -void btDbvt::write(IWriter* iwriter) const +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) + 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 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); + 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); - } + iwriter->WriteLeaf(n, i, p); + } } } // -void btDbvt::clone(btDbvt& dest,IClone* iclone) const +void btDbvt::clone(btDbvt& dest, IClone* iclone) const { dest.clear(); - if(m_root!=0) - { - btAlignedObjectArray<sStkCLN> stack; + 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.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; + if (e.parent != 0) + e.parent->childs[i & 1] = n; else - dest.m_root=n; - if(e.node->isinternal()) + 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)); + 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); + } while (stack.size() > 0); } } // -int btDbvt::maxdepth(const btDbvtNode* node) +int btDbvt::maxdepth(const btDbvtNode* node) { - int depth=0; - if(node) getmaxdepth(node,1,depth); - return(depth); + int depth = 0; + if (node) getmaxdepth(node, 1, depth); + return (depth); } // -int btDbvt::countLeaves(const btDbvtNode* node) +int btDbvt::countLeaves(const btDbvtNode* node) { - if(node->isinternal()) - return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + if (node->isinternal()) + return (countLeaves(node->childs[0]) + countLeaves(node->childs[1])); else - return(1); + return (1); } // -void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves) +void btDbvt::extractLeaves(const btDbvtNode* node, btAlignedObjectArray<const btDbvtNode*>& leaves) { - if(node->isinternal()) + if (node->isinternal()) { - extractLeaves(node->childs[0],leaves); - extractLeaves(node->childs[1],leaves); + extractLeaves(node->childs[0], leaves); + extractLeaves(node->childs[1], leaves); } else { leaves.push_back(node); - } + } } // @@ -694,603 +749,608 @@ 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) + 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); } + 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; + int m_pcount; + btScalar m_depth; + bool m_checksort; }; struct P14 : btDbvt::ICollide { struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; - void Process(const btDbvtNode* leaf,btScalar depth) + void Process(const btDbvtNode* leaf, btScalar depth) { - Node n; - n.leaf = leaf; - n.depth = depth; + Node n; + n.leaf = leaf; + n.depth = depth; } - static int sortfnc(const Node& a,const Node& b) + 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); + if (a.depth < b.depth) return (+1); + if (a.depth > b.depth) return (-1); + return (0); } - btAlignedObjectArray<Node> m_nodes; + btAlignedObjectArray<Node> m_nodes; }; struct P15 : btDbvt::ICollide { struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; void Process(const btDbvtNode* leaf) { - Node n; - n.leaf = leaf; - n.depth = dot(leaf->volume.Center(),m_axis); + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(), m_axis); } - static int sortfnc(const Node& a,const Node& b) + 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); + if (a.depth < b.depth) return (+1); + if (a.depth > b.depth) return (-1); + return (0); } - btAlignedObjectArray<Node> m_nodes; - btVector3 m_axis; + btAlignedObjectArray<Node> m_nodes; + btVector3 m_axis; }; - static btScalar RandUnit() + static btScalar RandUnit() { - return(rand()/(btScalar)RAND_MAX); + return (rand() / (btScalar)RAND_MAX); } - static btVector3 RandVector3() + static btVector3 RandVector3() { - return(btVector3(RandUnit(),RandUnit(),RandUnit())); + return (btVector3(RandUnit(), RandUnit(), RandUnit())); } - static btVector3 RandVector3(btScalar cs) + static btVector3 RandVector3(btScalar cs) { - return(RandVector3()*cs-btVector3(cs,cs,cs)/2); + return (RandVector3() * cs - btVector3(cs, cs, cs) / 2); } - static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) + static btDbvtVolume RandVolume(btScalar cs, btScalar eb, btScalar es) { - return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); + return (btDbvtVolume::FromCE(RandVector3(cs), btVector3(eb, eb, eb) + RandVector3() * es)); } - static btTransform RandTransform(btScalar cs) + static btTransform RandTransform(btScalar cs) { - btTransform t; + btTransform t; t.setOrigin(RandVector3(cs)); - t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); - return(t); + 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) + static void RandTree(btScalar cs, btScalar eb, btScalar es, int leaves, btDbvt& dbvt) { dbvt.clear(); - for(int i=0;i<leaves;++i) + for (int i = 0; i < leaves; ++i) { - dbvt.insert(RandVolume(cs,eb,es),0); + dbvt.insert(RandVolume(cs, eb, es), 0); } } }; -void btDbvt::benchmark() +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; + 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; + 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; + 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; + 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; + 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; + 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; + bool cfgBenchmark6_Enable = cfgEnable; + static const int cfgBenchmark6_Iterations = 512; + static const btScalar cfgBenchmark6_OffsetScale = 2; + static const int cfgBenchmark6_Reference = 7270; //[7] btDbvt::rayTest - bool cfgBenchmark7_Enable = cfgEnable; - static const int cfgBenchmark7_Passes = 32; - static const int cfgBenchmark7_Iterations = 65536; - static const int cfgBenchmark7_Reference = 6307; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + bool cfgBenchmark17_Enable = cfgEnable; + static const int cfgBenchmark17_Iterations = 4; + static const int cfgBenchmark17_Reference = 3390; - btClock wallclock; + 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 + 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; + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<bool> results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) + for (int i = 0; i < cfgLeaves; ++i) { - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); } printf("[1] btDbvtVolume intersections: "); wallclock.reset(); - for(int i=0;i<cfgBenchmark1_Iterations;++i) + for (int i = 0; i < cfgBenchmark1_Iterations; ++i) { - for(int j=0;j<cfgLeaves;++j) + for (int j = 0; j < cfgLeaves; ++j) { - for(int k=0;k<cfgLeaves;++k) + for (int k = 0; k < cfgLeaves; ++k) { - results[k]=Intersect(volumes[j],volumes[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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark1_Reference) * 100 / time); } - if(cfgBenchmark2_Enable) - {// Benchmark 2 + if (cfgBenchmark2_Enable) + { // Benchmark 2 srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<btDbvtVolume> results; + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<btDbvtVolume> results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) + for (int i = 0; i < cfgLeaves; ++i) { - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); } printf("[2] btDbvtVolume merges: "); wallclock.reset(); - for(int i=0;i<cfgBenchmark2_Iterations;++i) + for (int i = 0; i < cfgBenchmark2_Iterations; ++i) { - for(int j=0;j<cfgLeaves;++j) + for (int j = 0; j < cfgLeaves; ++j) { - for(int k=0;k<cfgLeaves;++k) + for (int k = 0; k < cfgLeaves; ++k) { - Merge(volumes[j],volumes[k],results[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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark2_Reference) * 100 / time); } - if(cfgBenchmark3_Enable) - {// Benchmark 3 + 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]); + 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) + for (int i = 0; i < cfgBenchmark3_Iterations; ++i) { - btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy); + 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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark3_Reference) * 100 / time); } - if(cfgBenchmark4_Enable) - {// Benchmark 4 + if (cfgBenchmark4_Enable) + { // Benchmark 4 srand(380843); - btDbvt dbvt; - btDbvtBenchmark::NilPolicy policy; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + 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) + for (int i = 0; i < cfgBenchmark4_Iterations; ++i) { - btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy); + 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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark4_Reference) * 100 / time); } - if(cfgBenchmark5_Enable) - {// Benchmark 5 + if (cfgBenchmark5_Enable) + { // Benchmark 5 srand(380843); - btDbvt dbvt[2]; - btAlignedObjectArray<btTransform> transforms; - btDbvtBenchmark::NilPolicy policy; + btDbvt dbvt[2]; + btAlignedObjectArray<btTransform> transforms; + btDbvtBenchmark::NilPolicy policy; transforms.resize(cfgBenchmark5_Iterations); - for(int i=0;i<transforms.size();++i) + for (int i = 0; i < transforms.size(); ++i) { - transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale); + transforms[i] = btDbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark5_OffsetScale); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); + 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) + for (int i = 0; i < cfgBenchmark5_Iterations; ++i) { - btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy); + 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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark5_Reference) * 100 / time); } - if(cfgBenchmark6_Enable) - {// Benchmark 6 + if (cfgBenchmark6_Enable) + { // Benchmark 6 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btTransform> transforms; - btDbvtBenchmark::NilPolicy policy; + btDbvt dbvt; + btAlignedObjectArray<btTransform> transforms; + btDbvtBenchmark::NilPolicy policy; transforms.resize(cfgBenchmark6_Iterations); - for(int i=0;i<transforms.size();++i) + for (int i = 0; i < transforms.size(); ++i) { - transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale); + transforms[i] = btDbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark6_OffsetScale); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + 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) + for (int i = 0; i < cfgBenchmark6_Iterations; ++i) { - btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy); + 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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark6_Reference) * 100 / time); } - if(cfgBenchmark7_Enable) - {// Benchmark 7 + if (cfgBenchmark7_Enable) + { // Benchmark 7 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> rayorg; - btAlignedObjectArray<btVector3> raydir; - btDbvtBenchmark::NilPolicy policy; + 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) + for (int i = 0; i < rayorg.size(); ++i) { - rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); - raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); + rayorg[i] = btDbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2); + raydir[i] = btDbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); dbvt.optimizeTopDown(); printf("[7] btDbvt::rayTest: "); wallclock.reset(); - for(int i=0;i<cfgBenchmark7_Passes;++i) + for (int i = 0; i < cfgBenchmark7_Passes; ++i) { - for(int j=0;j<cfgBenchmark7_Iterations;++j) + for (int j = 0; j < cfgBenchmark7_Iterations; ++j) { - btDbvt::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy); + btDbvt::rayTest(dbvt.m_root, rayorg[j], 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); + 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 + if (cfgBenchmark8_Enable) + { // Benchmark 8 srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + 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 i = 0; i < cfgBenchmark8_Passes; ++i) { - for(int j=0;j<cfgBenchmark8_Iterations;++j) + for (int j = 0; j < cfgBenchmark8_Iterations; ++j) { - dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0)); + 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); + 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 + if (cfgBenchmark9_Enable) + { // Benchmark 9 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<const btDbvtNode*> leaves; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + btDbvt dbvt; + btAlignedObjectArray<const btDbvtNode*> leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); + dbvt.extractLeaves(dbvt.m_root, leaves); printf("[9] updates (teleport): "); wallclock.reset(); - for(int i=0;i<cfgBenchmark9_Passes;++i) + for (int i = 0; i < cfgBenchmark9_Passes; ++i) { - for(int j=0;j<cfgBenchmark9_Iterations;++j) + for (int j = 0; j < cfgBenchmark9_Iterations; ++j) { - dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]), - btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + 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); + 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 + if (cfgBenchmark10_Enable) + { // Benchmark 10 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<const btDbvtNode*> leaves; - btAlignedObjectArray<btVector3> vectors; + btDbvt dbvt; + btAlignedObjectArray<const btDbvtNode*> leaves; + btAlignedObjectArray<btVector3> vectors; vectors.resize(cfgBenchmark10_Iterations); - for(int i=0;i<vectors.size();++i) + for (int i = 0; i < vectors.size(); ++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale; + vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)) * cfgBenchmark10_Scale; } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); + dbvt.extractLeaves(dbvt.m_root, leaves); printf("[10] updates (jitter): "); wallclock.reset(); - for(int i=0;i<cfgBenchmark10_Passes;++i) + 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); + 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); + 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 + if (cfgBenchmark11_Enable) + { // Benchmark 11 srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + 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) + 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); + 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 + if (cfgBenchmark12_Enable) + { // Benchmark 12 srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<bool> results; + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<bool> results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) + for (int i = 0; i < cfgLeaves; ++i) { - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); } printf("[12] btDbvtVolume notequal: "); wallclock.reset(); - for(int i=0;i<cfgBenchmark12_Iterations;++i) + for (int i = 0; i < cfgBenchmark12_Iterations; ++i) { - for(int j=0;j<cfgLeaves;++j) + for (int j = 0; j < cfgLeaves; ++j) { - for(int k=0;k<cfgLeaves;++k) + for (int k = 0; k < cfgLeaves; ++k) { - results[k]=NotEqual(volumes[j],volumes[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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark12_Reference) * 100 / time); } - if(cfgBenchmark13_Enable) - {// Benchmark 13 + if (cfgBenchmark13_Enable) + { // Benchmark 13 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> vectors; - btDbvtBenchmark::NilPolicy policy; + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::NilPolicy policy; vectors.resize(cfgBenchmark13_Iterations); - for(int i=0;i<vectors.size();++i) + for (int i = 0; i < vectors.size(); ++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)).normalized(); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); dbvt.optimizeTopDown(); printf("[13] culling(OCL+fullsort): "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark13_Iterations;++i) + 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); + 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); + 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 + if (cfgBenchmark14_Enable) + { // Benchmark 14 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> vectors; - btDbvtBenchmark::P14 policy; + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::P14 policy; vectors.resize(cfgBenchmark14_Iterations); - for(int i=0;i<vectors.size();++i) + for (int i = 0; i < vectors.size(); ++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)).normalized(); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + 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) + wallclock.reset(); + for (int i = 0; i < cfgBenchmark14_Iterations; ++i) { - static const btScalar offset=0; + static const btScalar offset = 0; policy.m_nodes.resize(0); - dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false); + 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); + 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 + if (cfgBenchmark15_Enable) + { // Benchmark 15 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> vectors; - btDbvtBenchmark::P15 policy; + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::P15 policy; vectors.resize(cfgBenchmark15_Iterations); - for(int i=0;i<vectors.size();++i) + for (int i = 0; i < vectors.size(); ++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)).normalized(); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + 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) + wallclock.reset(); + for (int i = 0; i < cfgBenchmark15_Iterations; ++i) { - static const btScalar offset=0; + 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_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); + 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 + if (cfgBenchmark16_Enable) + { // Benchmark 16 srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btDbvtNode*> batch; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + 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); + printf("[16] insert/remove batch(%u): ", cfgBenchmark16_BatchCount); wallclock.reset(); - for(int i=0;i<cfgBenchmark16_Passes;++i) + for (int i = 0; i < cfgBenchmark16_Passes; ++i) { - for(int j=0;j<cfgBenchmark16_BatchCount;++j) + for (int j = 0; j < cfgBenchmark16_BatchCount; ++j) { - batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0)); + batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0)); } - for(int j=0;j<cfgBenchmark16_BatchCount;++j) + 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)); + 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 + if (cfgBenchmark17_Enable) + { // Benchmark 17 srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<int> results; - btAlignedObjectArray<int> indices; + 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) + for (int i = 0; i < cfgLeaves; ++i) { - indices[i]=i; - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + indices[i] = i; + volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); } - for(int i=0;i<cfgLeaves;++i) + for (int i = 0; i < cfgLeaves; ++i) { - btSwap(indices[i],indices[rand()%cfgLeaves]); + btSwap(indices[i], indices[rand() % cfgLeaves]); } printf("[17] btDbvtVolume select: "); wallclock.reset(); - for(int i=0;i<cfgBenchmark17_Iterations;++i) + for (int i = 0; i < cfgBenchmark17_Iterations; ++i) { - for(int j=0;j<cfgLeaves;++j) + for (int j = 0; j < cfgLeaves; ++j) { - for(int k=0;k<cfgLeaves;++k) + for (int k = 0; k < cfgLeaves; ++k) { - const int idx=indices[k]; - results[idx]=Select(volumes[idx],volumes[j],volumes[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); + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark17_Reference) * 100 / time); } printf("\r\n\r\n"); } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h index db4e482f292..55daa7fb578 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -21,55 +21,53 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btAabbUtil2.h" - // // Compile time configuration // - // Implementation profiles -#define DBVT_IMPL_GENERIC 0 // Generic implementation -#define DBVT_IMPL_SSE 1 // SSE +#define DBVT_IMPL_GENERIC 0 // Generic implementation +#define DBVT_IMPL_SSE 1 // SSE // Template implementation of ICollide #ifdef _WIN32 -#if (defined (_MSC_VER) && _MSC_VER >= 1400) -#define DBVT_USE_TEMPLATE 1 +#if (defined(_MSC_VER) && _MSC_VER >= 1400) +#define DBVT_USE_TEMPLATE 1 #else -#define DBVT_USE_TEMPLATE 0 +#define DBVT_USE_TEMPLATE 0 #endif #else -#define DBVT_USE_TEMPLATE 0 +#define DBVT_USE_TEMPLATE 0 #endif // Use only intrinsics instead of inline asm -#define DBVT_USE_INTRINSIC_SSE 1 +#define DBVT_USE_INTRINSIC_SSE 1 // Using memmov for collideOCL -#define DBVT_USE_MEMMOVE 1 +#define DBVT_USE_MEMMOVE 1 // Enable benchmarking code -#define DBVT_ENABLE_BENCHMARK 0 +#define DBVT_ENABLE_BENCHMARK 0 // Inlining -#define DBVT_INLINE SIMD_FORCE_INLINE +#define DBVT_INLINE SIMD_FORCE_INLINE // Specific methods implementation //SSE gives errors on a MSVC 7.1 -#if defined (BT_USE_SSE) //&& defined (_WIN32) -#define DBVT_SELECT_IMPL DBVT_IMPL_SSE -#define DBVT_MERGE_IMPL DBVT_IMPL_SSE -#define DBVT_INT0_IMPL DBVT_IMPL_SSE +#if defined(BT_USE_SSE) //&& defined (_WIN32) +#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 +#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) +#if (DBVT_SELECT_IMPL == DBVT_IMPL_SSE) || \ + (DBVT_MERGE_IMPL == DBVT_IMPL_SSE) || \ + (DBVT_INT0_IMPL == DBVT_IMPL_SSE) #include <emmintrin.h> #endif @@ -78,21 +76,24 @@ subject to the following restrictions: // #if DBVT_USE_TEMPLATE -#define DBVT_VIRTUAL +#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*)1;(void)typechecker; +#define DBVT_PREFIX template <typename T> +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE \ + static const ICollide& typechecker = *(T*)1; \ + (void)typechecker; #else -#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} -#define DBVT_VIRTUAL virtual +#define DBVT_VIRTUAL_DTOR(a) \ + virtual ~a() {} +#define DBVT_VIRTUAL virtual #define DBVT_PREFIX -#define DBVT_IPOLICY ICollide& policy +#define DBVT_IPOLICY ICollide& policy #define DBVT_CHECKTYPE #endif #if DBVT_USE_MEMMOVE -#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__) #include <memory.h> #endif #include <string.h> @@ -126,187 +127,235 @@ subject to the following restrictions: // Defaults volumes // -/* btDbvtAabbMm */ -struct btDbvtAabbMm +/* 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 btVector3& b); - - 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); - - DBVT_INLINE btVector3& tMins() { return(mi); } - DBVT_INLINE btVector3& tMaxs() { return(mx); } - + DBVT_INLINE 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 btVector3& b); + + 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); + + DBVT_INLINE btVector3& tMins() { return (mi); } + DBVT_INLINE btVector3& tMaxs() { return (mx); } + private: - DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; + DBVT_INLINE void AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const; + private: - btVector3 mi,mx; + btVector3 mi, mx; }; -// Types -typedef btDbvtAabbMm btDbvtVolume; +// Types +typedef btDbvtAabbMm btDbvtVolume; -/* btDbvtNode */ -struct btDbvtNode +/* 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; + 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; }; }; +/* btDbv(normal)tNode */ +struct btDbvntNode +{ + btDbvtVolume volume; + btVector3 normal; + btScalar angle; + DBVT_INLINE bool isleaf() const { return (childs[1] == 0); } + DBVT_INLINE bool isinternal() const { return (!isleaf()); } + btDbvntNode* childs[2]; + void* data; + + btDbvntNode(const btDbvtNode* n) + : volume(n->volume) + , normal(0,0,0) + , angle(0) + , data(n->data) + { + childs[0] = 0; + childs[1] = 0; + } + + ~btDbvntNode() + { + if (childs[0]) + delete childs[0]; + if (childs[1]) + delete childs[1]; + } +}; + +typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack; + ///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 +struct btDbvt { - /* Stack element */ - struct sStkNN + /* Stack element */ + struct sStkNN { - const btDbvtNode* a; - const btDbvtNode* b; + const btDbvtNode* a; + const btDbvtNode* b; sStkNN() {} - sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} + sStkNN(const btDbvtNode* na, const btDbvtNode* nb) : a(na), b(nb) {} }; - struct sStkNP + struct sStkNP { - const btDbvtNode* node; - int mask; - sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} + const btDbvtNode* node; + int mask; + sStkNP(const btDbvtNode* n, unsigned m) : node(n), mask(m) {} }; - struct sStkNPS + struct sStkNPS { - const btDbvtNode* node; - int mask; - btScalar value; + const btDbvtNode* node; + int mask; + btScalar value; sStkNPS() {} - sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} + sStkNPS(const btDbvtNode* n, unsigned m, btScalar v) : node(n), mask(m), value(v) {} }; - struct sStkCLN + struct sStkCLN { - const btDbvtNode* node; - btDbvtNode* parent; - sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} + const btDbvtNode* node; + btDbvtNode* parent; + sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {} }; + + struct sStknNN + { + const btDbvntNode* a; + const btDbvntNode* b; + sStknNN() {} + sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {} + }; // Policies/Interfaces - /* ICollide */ - struct ICollide - { + /* 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); } + 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 void Process(const btDbvntNode*, const btDbvntNode*) {} + DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); } + DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); } }; - /* IWriter */ - struct IWriter + /* 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; + 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 + /* IClone */ + struct IClone { - virtual ~IClone() {} - virtual void CloneLeaf(btDbvtNode*) {} + virtual ~IClone() {} + virtual void CloneLeaf(btDbvtNode*) {} }; // Constants - enum { - SIMPLE_STACKSIZE = 64, - DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + 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; - - - btAlignedObjectArray<sStkNN> m_stkStack; - mutable btAlignedObjectArray<const btDbvtNode*> m_rayTestStack; + btDbvtNode* m_root; + btDbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + btAlignedObjectArray<sStkNN> m_stkStack; // 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,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); + 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, 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(); + static void benchmark(); #else - static void benchmark(){} + static void benchmark() + { + } #endif // DBVT_IPOLICY must support ICollide policy/interface DBVT_PREFIX - static void enumNodes( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumNodes(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumLeaves(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); + DBVT_PREFIX + void selfCollideT(const btDbvntNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + void selfCollideTT(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - void collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTTpersistentStack(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); #if 0 DBVT_PREFIX void collideTT( const btDbvtNode* root0, @@ -322,71 +371,89 @@ struct btDbvt #endif DBVT_PREFIX - void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - DBVT_IPOLICY) const; + void collideTV(const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY) const; + + DBVT_PREFIX + void collideTVNoStackAlloc(const btDbvtNode* root, + const btDbvtVolume& volume, + btNodeStack& stack, + DBVT_IPOLICY) const; + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time DBVT_PREFIX - static void rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY); + static void rayTest(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY); ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts DBVT_PREFIX - void rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - DBVT_IPOLICY) const; + void rayTestInternal(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + btAlignedObjectArray<const btDbvtNode*>& stack, + DBVT_IPOLICY) const; DBVT_PREFIX - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); + 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); + 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) + 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) + int m = 0; + while (l < h) { - m=(l+h)>>1; - if(a[i[m]].value>=v) l=m+1; else h=m; + m = (l + h) >> 1; + if (a[i[m]].value >= v) + l = m + 1; + else + h = m; } - return(h); + return (h); } - static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree, - btAlignedObjectArray<sStkNPS>& stock, - const sStkNPS& value) + 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; } + 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); + { + i = stock.size(); + stock.push_back(value); + } + return (i); } // private: - btDbvt(const btDbvt&) {} + btDbvt(const btDbvt&) {} }; // @@ -394,227 +461,252 @@ private: // // -inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c, const btVector3& e) { btDbvtAabbMm box; - box.mi=c-e;box.mx=c+e; - return(box); + box.mi = c - e; + box.mx = c + e; + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c, btScalar r) { - return(FromCE(c,btVector3(r,r,r))); + return (FromCE(c, btVector3(r, r, r))); } // -inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi, const btVector3& mx) { btDbvtAabbMm box; - box.mi=mi;box.mx=mx; - return(box); + box.mi = mi; + box.mx = mx; + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) +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 = box.mx = pts[0]; + for (int i = 1; i < n; ++i) { box.mi.setMin(pts[i]); box.mx.setMax(pts[i]); } - return(box); + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n) +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 = box.mx = *ppts[0]; + for (int i = 1; i < n; ++i) { box.mi.setMin(*ppts[i]); box.mx.setMax(*ppts[i]); } - return(box); + return (box); } // -DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) +DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) { - mi-=e;mx+=e; + mi -= e; + mx += e; } // -DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& 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]); + 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 +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())); + 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 +DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n, btScalar o, int s) const { - btVector3 pi,px; - switch(s) + 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; + 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((btDot(n,px)+o)<0) return(-1); - if((btDot(n,pi)+o)>=0) return(+1); - return(0); + if ((btDot(n, px) + o) < 0) return (-1); + if ((btDot(n, pi) + o) >= 0) return (+1); + return (0); } // -DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const +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(btDot(p,v)); + const btVector3* b[] = {&mx, &mi}; + const btVector3 p(b[(signs >> 0) & 1]->x(), + b[(signs >> 1) & 1]->y(), + b[(signs >> 2) & 1]->z()); + return (btDot(p, v)); } // -DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const +DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const { - for(int i=0;i<3;++i) + for (int i = 0; i < 3; ++i) { - if(d[i]<0) - { smi+=mx[i]*d[i];smx+=mi[i]*d[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]; } + { + smi += mi[i] * d[i]; + smx += mx[i] * d[i]; + } } } // -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +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)))); -#if defined (_WIN32) - const __int32* pu((const __int32*)&rt); +#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)))); +#if defined(_WIN32) + const __int32* pu((const __int32*)&rt); #else - const int* pu((const int*)&rt); + const int* pu((const int*)&rt); #endif - return((pu[0]|pu[1]|pu[2])==0); + 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())); + 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 btVector3& b) +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())); + 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 btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +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())); + 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) +DBVT_INLINE int Select(const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { -#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE - -#if defined (_WIN32) - static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; +#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE + +#if defined(_WIN32) + static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff}; #else - static ATTRIBUTE_ALIGNED16(const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/}; + static ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/}; #endif ///@todo: the intrinsic version is 11% slower #if DBVT_USE_INTRINSIC_SSE - union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory + union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory { - __m128 ssereg; - float floats[4]; - int ints[4]; + __m128 ssereg; + float floats[4]; + int ints[4]; }; - __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)); - + __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)); + btSSEUnion tmp; - tmp.ssereg = _mm_cmple_ss(bmi,ami); - return tmp.ints[0]&1; + tmp.ssereg = _mm_cmple_ss(bmi, ami); + return tmp.ints[0] & 1; #else - ATTRIBUTE_ALIGNED16(__int32 r[1]); + ATTRIBUTE_ALIGNED16(__int32 r[1]); __asm { mov eax,o @@ -642,46 +734,52 @@ DBVT_INLINE int Select( const btDbvtAabbMm& o, cmpless xmm2,xmm1 movss r,xmm2 } - return(r[0]&1); + return (r[0] & 1); #endif #else - return(Proximity(o,a)<Proximity(o,b)?0:1); + return (Proximity(o, a) < Proximity(o, b) ? 0 : 1); #endif } // -DBVT_INLINE void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r) +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); +#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) + 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]; + 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) +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())); + 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())); } // @@ -690,162 +788,291 @@ DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, // DBVT_PREFIX -inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::enumNodes(const btDbvtNode* root, + DBVT_IPOLICY) { DBVT_CHECKTYPE - policy.Process(root); - if(root->isinternal()) + policy.Process(root); + if (root->isinternal()) { - enumNodes(root->childs[0],policy); - enumNodes(root->childs[1],policy); + enumNodes(root->childs[0], policy); + enumNodes(root->childs[1], policy); } } // DBVT_PREFIX -inline void btDbvt::enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY) +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); - } + 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) +inline void btDbvt::collideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray<sStkNN> stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStkNN(root0, root1); + do { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - btAlignedObjectArray<sStkNN> stkStack; - stkStack.resize(DOUBLE_STACKSIZE); - stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=stkStack[--depth]; - if(depth>treshold) + sStkNN p = stkStack[--depth]; + if (depth > treshold) + { + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal()) { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; + stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]); } - if(p.a==p.b) + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) { - if(p.a->isinternal()) + if (p.b->isinternal()) + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]); + } + else { - stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b); } } - else if(Intersect(p.a->volume,p.b->volume)) + else { - if(p.a->isinternal()) + if (p.b->isinternal()) { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } + stkStack[depth++] = sStkNN(p.a, p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a, p.b->childs[1]); } else { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } + policy.Process(p.a, p.b); } } - } while(depth); - } + } + } while (depth); + } +} + +// +DBVT_PREFIX +inline void btDbvt::selfCollideT(const btDbvntNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if (root) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray<sStknNN> stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStknNN(root, root); + do + { + sStknNN p = stkStack[--depth]; + if (depth > treshold) + { + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal() && p.a->angle > SIMD_PI) + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]); + stkStack[depth++] = sStknNN(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()) + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]); + } + else + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.b); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b); + } + } + else + { + if (p.b->isinternal()) + { + stkStack[depth++] = sStknNN(p.a, p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a, p.b->childs[1]); + } + else + { + policy.Process(p.a, p.b); + } + } + } + } while (depth); + } } +// +DBVT_PREFIX +inline void btDbvt::selfCollideTT(const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if (root) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray<sStkNN> stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStkNN(root, root); + do + { + sStkNN p = stkStack[--depth]; + if (depth > treshold) + { + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal()) + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]); + stkStack[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()) + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]); + } + else + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b); + } + } + else + { + if (p.b->isinternal()) + { + stkStack[depth++] = sStkNN(p.a, p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a, p.b->childs[1]); + } + else + { + policy.Process(p.a, p.b); + } + } + } + } while (depth); + } +} DBVT_PREFIX -inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) +inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + + m_stkStack.resize(DOUBLE_STACKSIZE); + m_stkStack[0] = sStkNN(root0, root1); + do { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - - m_stkStack.resize(DOUBLE_STACKSIZE); - m_stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=m_stkStack[--depth]; - if(depth>treshold) + sStkNN p = m_stkStack[--depth]; + if (depth > treshold) + { + m_stkStack.resize(m_stkStack.size() * 2); + treshold = m_stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal()) { - m_stkStack.resize(m_stkStack.size()*2); - treshold=m_stkStack.size()-4; + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]); + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]); } - if(p.a==p.b) + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) { - if(p.a->isinternal()) + if (p.b->isinternal()) { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]); + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]); + } + else + { + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b); } } - else if(Intersect(p.a->volume,p.b->volume)) + else { - if(p.a->isinternal()) + if (p.b->isinternal()) { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } + m_stkStack[depth++] = sStkNN(p.a, p.b->childs[0]); + m_stkStack[depth++] = sStkNN(p.a, p.b->childs[1]); } else { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } + policy.Process(p.a, p.b); } } - } while(depth); - } + } + } while (depth); + } } #if 0 @@ -915,353 +1142,419 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, const btTransform xform=xform0.inverse()*xform1; collideTT(root0,root1,xform,policy); } -#endif +#endif + +DBVT_PREFIX +inline void btDbvt::collideTV(const btDbvtNode* root, + const btDbvtVolume& vol, + DBVT_IPOLICY) const +{ + DBVT_CHECKTYPE + if (root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume(vol); + btAlignedObjectArray<const btDbvtNode*> stack; + stack.resize(0); +#ifndef BT_DISABLE_STACK_TEMP_MEMORY + char tempmemory[SIMPLE_STACKSIZE * sizeof(const btDbvtNode*)]; + stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE); +#else + stack.reserve(SIMPLE_STACKSIZE); +#endif //BT_DISABLE_STACK_TEMP_MEMORY + + 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::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) const +inline void btDbvt::collideTVNoStackAlloc(const btDbvtNode* root, + const btDbvtVolume& vol, + btNodeStack& stack, + DBVT_IPOLICY) const { DBVT_CHECKTYPE - if(root) + if (root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume(vol); + stack.resize(0); + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray<const btDbvtNode*> stack; - stack.resize(0); - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->volume,volume)) + const btDbvtNode* n = stack[stack.size() - 1]; + stack.pop_back(); + if (Intersect(n->volume, volume)) + { + if (n->isinternal()) { - if(n->isinternal()) - { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); - } - else - { - policy.Process(n); - } + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); } - } while(stack.size()>0); - } + else + { + policy.Process(n); + } + } + } while (stack.size() > 0); + } } DBVT_PREFIX -inline void btDbvt::rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - DBVT_IPOLICY) const +inline void btDbvt::rayTestInternal(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + btAlignedObjectArray<const btDbvtNode*>& stack, + DBVT_IPOLICY) const { - (void) rayTo; + (void)rayTo; DBVT_CHECKTYPE - if(root) + if (root) { btVector3 resultNormal; - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; - btAlignedObjectArray<const btDbvtNode*>& stack = m_rayTestStack; + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 2; stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; + stack[0] = root; btVector3 bounds[2]; - do + do { - const btDbvtNode* node=stack[--depth]; - bounds[0] = node->volume.Mins()-aabbMax; - bounds[1] = node->volume.Maxs()-aabbMin; - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1=false; - result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); - if(result1) + const btDbvtNode* node = stack[--depth]; + bounds[0] = node->volume.Mins() - aabbMax; + bounds[1] = node->volume.Maxs() - aabbMin; + btScalar tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = false; + result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max); + if (result1) { - if(node->isinternal()) + if (node->isinternal()) { - if(depth>treshold) + if (depth > treshold) { - stack.resize(stack.size()*2); - treshold=stack.size()-2; + stack.resize(stack.size() * 2); + treshold = stack.size() - 2; } - stack[depth++]=node->childs[0]; - stack[depth++]=node->childs[1]; + stack[depth++] = node->childs[0]; + stack[depth++] = node->childs[1]; } else { policy.Process(node); } } - } while(depth); + } while (depth); } } // DBVT_PREFIX -inline void btDbvt::rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY) +inline void btDbvt::rayTest(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root) - { - btVector3 rayDir = (rayTo-rayFrom); - rayDir.normalize (); + if (root) + { + btVector3 rayDir = (rayTo - rayFrom); + rayDir.normalize(); - ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT - btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; + unsigned int signs[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; - btScalar lambda_max = rayDir.dot(rayTo-rayFrom); + btScalar lambda_max = rayDir.dot(rayTo - rayFrom); - btVector3 resultNormal; + btVector3 resultNormal; - btAlignedObjectArray<const btDbvtNode*> stack; + btAlignedObjectArray<const btDbvtNode*> stack; - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 2; - stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; - btVector3 bounds[2]; - do { - const btDbvtNode* node=stack[--depth]; + char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)]; +#ifndef BT_DISABLE_STACK_TEMP_MEMORY + stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE); +#else //BT_DISABLE_STACK_TEMP_MEMORY + stack.resize(DOUBLE_STACKSIZE); +#endif //BT_DISABLE_STACK_TEMP_MEMORY + stack[0] = root; + btVector3 bounds[2]; + do + { + const btDbvtNode* node = stack[--depth]; - bounds[0] = node->volume.Mins(); - bounds[1] = node->volume.Maxs(); - - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); + + btScalar tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max); #ifdef COMPARE_BTRAY_AABB2 - btScalar param=1.f; - bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); - btAssert(result1 == result2); -#endif //TEST_BTRAY_AABB2 + btScalar param = 1.f; + bool result2 = btRayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal); + btAssert(result1 == result2); +#endif //TEST_BTRAY_AABB2 - if(result1) + if (result1) + { + if (node->isinternal()) { - if(node->isinternal()) - { - if(depth>treshold) - { - stack.resize(stack.size()*2); - treshold=stack.size()-2; - } - stack[depth++]=node->childs[0]; - stack[depth++]=node->childs[1]; - } - else + if (depth > treshold) { - policy.Process(node); + stack.resize(stack.size() * 2); + treshold = stack.size() - 2; } + stack[depth++] = node->childs[0]; + stack[depth++] = node->childs[1]; } - } while(depth); - - } + else + { + policy.Process(node); + } + } + } while (depth); + } } // DBVT_PREFIX -inline void btDbvt::collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY) +inline void btDbvt::collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root) + 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 { - 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) + sStkNP se = stack[stack.size() - 1]; + bool out = false; + stack.pop_back(); + for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1) { - 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)) { - if(0==(se.mask&j)) + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) { - 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; - } + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; } } - if(!out) + } + if (!out) + { + if ((se.mask != inside) && (se.node->isinternal())) { - 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); - } + stack.push_back(sStkNP(se.node->childs[0], se.mask)); + stack.push_back(sStkNP(se.node->childs[1], se.mask)); } - } while(stack.size()); - } + 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) +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) + 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 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) + const int id = stack[stack.size() - 1]; + sStkNPS se = stock[id]; + stack.pop_back(); + ifree.push_back(id); + if (se.mask != inside) { - 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) { - bool out=false; - for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + if (0 == (se.mask & j)) { - if(0==(se.mask&j)) + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) { - 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; - } + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; } } - if(out) continue; } - if(policy.Descent(se.node)) + if (out) continue; + } + if (policy.Descent(se.node)) + { + if (se.node->isinternal()) { - 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)) { - 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); - - //void * memmove ( void * destination, const void * source, size_t num ); - + /* Insert 0 */ + j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size()); + stack.push_back(0); + + //void * memmove ( void * destination, const void * source, size_t num ); + #if DBVT_USE_MEMMOVE - { - int num_items_to_move = stack.size()-1-j; - if(num_items_to_move > 0) - memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move); - } + { + int num_items_to_move = stack.size() - 1 - j; + if (num_items_to_move > 0) + memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move); + } #else - for(int k=stack.size()-1;k>j;--k) { - stack[k]=stack[k-1]; - } + 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); + 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 - { - int num_items_to_move = stack.size()-1-j; - if(num_items_to_move > 0) - memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move); - } -#else - for(int k=stack.size()-1;k>j;--k) { - stack[k]=stack[k-1]; - } -#endif - stack[j]=allocate(ifree,stock,nes[1-q]); + { + int num_items_to_move = stack.size() - 1 - j; + if (num_items_to_move > 0) + memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move); } - else +#else + for (int k = stack.size() - 1; k > j; --k) { - stack.push_back(allocate(ifree,stock,nes[q])); - stack.push_back(allocate(ifree,stock,nes[1-q])); + stack[k] = stack[k - 1]; } +#endif + stack[j] = allocate(ifree, stock, nes[1 - q]); } else { - policy.Process(se.node,se.value); + stack.push_back(allocate(ifree, stock, nes[q])); + stack.push_back(allocate(ifree, stock, nes[1 - q])); } } - } while(stack.size()); - } + else + { + policy.Process(se.node, se.value); + } + } + } while (stack.size()); + } } // DBVT_PREFIX -inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::collideTU(const btDbvtNode* root, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root) + if (root) + { + btAlignedObjectArray<const btDbvtNode*> stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { - 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)) + const btDbvtNode* n = stack[stack.size() - 1]; + stack.pop_back(); + if (policy.Descent(n)) + { + if (n->isinternal()) { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { policy.Process(n); } + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); } - } while(stack.size()>0); - } + else + { + policy.Process(n); + } + } + } while (stack.size() > 0); + } } // diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 75cfac64368..7b39dbdc0fe 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -16,33 +16,33 @@ subject to the following restrictions: ///btDbvtBroadphase implementation by Nathanael Presson #include "btDbvtBroadphase.h" - +#include "LinearMath/btThreads.h" +btScalar gDbvtMargin = btScalar(0.05); // // Profiling // -#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK +#if DBVT_BP_PROFILE || DBVT_BP_ENABLE_BENCHMARK #include <stdio.h> #endif #if DBVT_BP_PROFILE -struct ProfileScope +struct ProfileScope { - __forceinline ProfileScope(btClock& clock,unsigned long& value) : - m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + __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; + (*m_value) += m_clock->getTimeMicroseconds() - m_base; } - btClock* m_clock; - unsigned long* m_value; - unsigned long m_base; + btClock* m_clock; + unsigned long* m_value; + unsigned long m_base; }; -#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) +#define SPC(_value_) ProfileScope spc_scope(m_clock, _value_) #else -#define SPC(_value_) +#define SPC(_value_) #endif // @@ -51,66 +51,75 @@ struct ProfileScope // template <typename T> -static inline void listappend(T* item,T*& list) +static inline void listappend(T* item, T*& list) { - item->links[0]=0; - item->links[1]=list; - if(list) list->links[0]=item; - list=item; + 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) +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]; + 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) +static inline int listcount(T* root) { - int n=0; - while(root) { ++n;root=root->links[1]; } - return(n); + int n = 0; + while (root) + { + ++n; + root = root->links[1]; + } + return (n); } // template <typename T> -static inline void clear(T& value) +static inline void clear(T& value) { - static const struct ZeroDummy : T {} zerodummy; - value=zerodummy; + static const struct ZeroDummy : T + { + } zerodummy; + value = zerodummy; } // // Colliders // -/* Tree collider */ -struct btDbvtTreeCollider : btDbvt::ICollide +/* Tree collider */ +struct btDbvtTreeCollider : btDbvt::ICollide { - btDbvtBroadphase* pbp; - btDbvtProxy* proxy; + btDbvtBroadphase* pbp; + btDbvtProxy* proxy; btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} - void Process(const btDbvtNode* na,const btDbvtNode* nb) + void Process(const btDbvtNode* na, const btDbvtNode* nb) { - if(na!=nb) + if (na != nb) { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; + btDbvtProxy* pa = (btDbvtProxy*)na->data; + btDbvtProxy* pb = (btDbvtProxy*)nb->data; #if DBVT_BP_SORTPAIRS - if(pa->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); + if (pa->m_uniqueId > pb->m_uniqueId) + btSwap(pa, pb); #endif - pbp->m_paircache->addOverlappingPair(pa,pb); + pbp->m_paircache->addOverlappingPair(pa, pb); ++pbp->m_newpairs; } } - void Process(const btDbvtNode* n) + void Process(const btDbvtNode* n) { - Process(n,proxy->leaf); + Process(n, proxy->leaf); } }; @@ -121,27 +130,32 @@ struct btDbvtTreeCollider : btDbvt::ICollide // btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) { - m_deferedcollide = false; - m_needcleanup = true; - m_releasepaircache = (paircache!=0)?false:true; - m_prediction = 0; - 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_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache != 0) ? false : true; + m_prediction = 0; + 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; + m_stageRoots[i] = 0; } +#if BT_THREADSAFE + m_rayTestStacks.resize(BT_MAX_THREAD_COUNT); +#else + m_rayTestStacks.resize(1); +#endif #if DBVT_BP_PROFILE clear(m_profiling); #endif @@ -150,7 +164,7 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) // btDbvtBroadphase::~btDbvtBroadphase() { - if(m_releasepaircache) + if (m_releasepaircache) { m_paircache->~btOverlappingPairCache(); btAlignedFree(m_paircache); @@ -158,287 +172,294 @@ btDbvtBroadphase::~btDbvtBroadphase() } // -btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, - const btVector3& aabbMax, - int /*shapeType*/, - void* userPtr, - short int collisionFilterGroup, - short int collisionFilterMask, - btDispatcher* /*dispatcher*/, - void* /*multiSapProxy*/) +btBroadphaseProxy* btDbvtBroadphase::createProxy(const btVector3& aabbMin, + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + int collisionFilterGroup, + int collisionFilterMask, + btDispatcher* /*dispatcher*/) { - btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, - collisionFilterGroup, - collisionFilterMask); + btDbvtProxy* proxy = new (btAlignedAlloc(sizeof(btDbvtProxy), 16)) btDbvtProxy(aabbMin, aabbMax, userPtr, + collisionFilterGroup, + collisionFilterMask); - btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin, aabbMax); //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); - proxy->stage = m_stageCurrent; - proxy->m_uniqueId = ++m_gid; - proxy->leaf = m_sets[0].insert(aabb,proxy); - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(!m_deferedcollide) + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = ++m_gid; + proxy->leaf = m_sets[0].insert(aabb, proxy); + listappend(proxy, m_stageRoots[m_stageCurrent]); + if (!m_deferedcollide) { - btDbvtTreeCollider collider(this); - collider.proxy=proxy; - m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); - m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); + btDbvtTreeCollider collider(this); + collider.proxy = proxy; + m_sets[0].collideTV(m_sets[0].m_root, aabb, collider); + m_sets[1].collideTV(m_sets[1].m_root, aabb, collider); } - return(proxy); + return (proxy); } // -void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, - btDispatcher* dispatcher) +void btDbvtBroadphase::destroyProxy(btBroadphaseProxy* absproxy, + btDispatcher* dispatcher) { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - if(proxy->stage==STAGECOUNT) + 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); + listremove(proxy, m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy, dispatcher); btAlignedFree(proxy); - m_needcleanup=true; + m_needcleanup = true; } -void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const +void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy, btVector3& aabbMin, btVector3& aabbMax) const { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + btDbvtProxy* proxy = (btDbvtProxy*)absproxy; aabbMin = proxy->m_aabbMin; aabbMax = proxy->m_aabbMax; } -struct BroadphaseRayTester : btDbvt::ICollide +struct BroadphaseRayTester : btDbvt::ICollide { btBroadphaseRayCallback& m_rayCallback; BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) - :m_rayCallback(orgCallback) + : m_rayCallback(orgCallback) { } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + btDbvtProxy* proxy = (btDbvtProxy*)leaf->data; m_rayCallback.process(proxy); } -}; +}; -void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) +void btDbvtBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax) { BroadphaseRayTester callback(rayCallback); + btAlignedObjectArray<const btDbvtNode*>* stack = &m_rayTestStacks[0]; +#if BT_THREADSAFE + // for this function to be threadsafe, each thread must have a separate copy + // of this stack. This could be thread-local static to avoid dynamic allocations, + // instead of just a local. + int threadIndex = btGetCurrentThreadIndex(); + btAlignedObjectArray<const btDbvtNode*> localStack; + //todo(erwincoumans, "why do we get tsan issue here?") + if (0)//threadIndex < m_rayTestStacks.size()) + //if (threadIndex < m_rayTestStacks.size()) + { + // use per-thread preallocated stack if possible to avoid dynamic allocations + stack = &m_rayTestStacks[threadIndex]; + } + else + { + stack = &localStack; + } +#endif - m_sets[0].rayTestInternal( m_sets[0].m_root, - rayFrom, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - callback); - - m_sets[1].rayTestInternal( m_sets[1].m_root, - rayFrom, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - callback); - + m_sets[0].rayTestInternal(m_sets[0].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + *stack, + callback); + + m_sets[1].rayTestInternal(m_sets[1].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + *stack, + callback); } - -struct BroadphaseAabbTester : btDbvt::ICollide +struct BroadphaseAabbTester : btDbvt::ICollide { btBroadphaseAabbCallback& m_aabbCallback; BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback) - :m_aabbCallback(orgCallback) + : m_aabbCallback(orgCallback) { } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + btDbvtProxy* proxy = (btDbvtProxy*)leaf->data; m_aabbCallback.process(proxy); } -}; +}; -void btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback) +void btDbvtBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& aabbCallback) { BroadphaseAabbTester callback(aabbCallback); - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax); - //process all children, that overlap with the given AABB bounds - m_sets[0].collideTV(m_sets[0].m_root,bounds,callback); - m_sets[1].collideTV(m_sets[1].m_root,bounds,callback); - + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(aabbMin, aabbMax); + //process all children, that overlap with the given AABB bounds + m_sets[0].collideTV(m_sets[0].m_root, bounds, callback); + m_sets[1].collideTV(m_sets[1].m_root, bounds, callback); } - - // -void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) +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); + btDbvtProxy* proxy = (btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + aabb = btDbvtVolume::FromMM(aabbMin, aabbMax); #if DBVT_BP_PREVENTFALSEUPDATE - if(NotEqual(aabb,proxy->leaf->volume)) + if (NotEqual(aabb, proxy->leaf->volume)) #endif { - bool docollide=false; - if(proxy->stage==STAGECOUNT) - {/* fixed -> dynamic set */ + 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; + proxy->leaf = m_sets[0].insert(aabb, proxy); + docollide = true; } else - {/* dynamic set */ + { /* dynamic set */ ++m_updates_call; - if(Intersect(proxy->leaf->volume,aabb)) - {/* Moving */ - - const btVector3 delta=aabbMin-proxy->m_aabbMin; - btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*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 - ) + if (Intersect(proxy->leaf->volume, aabb)) + { /* Moving */ + + const btVector3 delta = aabbMin - proxy->m_aabbMin; + btVector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * 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 ( + m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin) + + ) { ++m_updates_done; - docollide=true; + docollide = true; } } else - {/* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); + { /* Teleporting */ + m_sets[0].update(proxy->leaf, aabb); ++m_updates_done; - docollide=true; - } + docollide = true; + } } - listremove(proxy,m_stageRoots[proxy->stage]); + listremove(proxy, m_stageRoots[proxy->stage]); proxy->m_aabbMin = aabbMin; proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) + proxy->stage = m_stageCurrent; + listappend(proxy, m_stageRoots[m_stageCurrent]); + if (docollide) { - m_needcleanup=true; - if(!m_deferedcollide) + m_needcleanup = true; + if (!m_deferedcollide) { - btDbvtTreeCollider collider(this); - m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider); } - } + } } } - // -void btDbvtBroadphase::setAabbForceUpdate( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) +void btDbvtBroadphase::setAabbForceUpdate(btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); - bool docollide=false; - if(proxy->stage==STAGECOUNT) - {/* fixed -> dynamic set */ + btDbvtProxy* proxy = (btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + aabb = btDbvtVolume::FromMM(aabbMin, aabbMax); + 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; + proxy->leaf = m_sets[0].insert(aabb, proxy); + docollide = true; } else - {/* dynamic set */ + { /* dynamic set */ ++m_updates_call; - /* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); + /* Teleporting */ + m_sets[0].update(proxy->leaf, aabb); ++m_updates_done; - docollide=true; + docollide = true; } - listremove(proxy,m_stageRoots[proxy->stage]); + listremove(proxy, m_stageRoots[proxy->stage]); proxy->m_aabbMin = aabbMin; proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) + proxy->stage = m_stageCurrent; + listappend(proxy, m_stageRoots[m_stageCurrent]); + if (docollide) { - m_needcleanup=true; - if(!m_deferedcollide) + m_needcleanup = true; + if (!m_deferedcollide) { - btDbvtTreeCollider collider(this); - m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider); } - } + } } // -void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +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)); + 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 performDeferredRemoval(dispatcher); - } void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) { - if (m_paircache->hasDeferredRemoval()) { - - btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); + btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); //perform a sort, to find duplicates and to sort 'invalid' pairs to the end overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); int 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++) + + for (i = 0; i < overlappingPairArray.size(); i++) { - btBroadphasePair& pair = overlappingPairArray[i]; bool isDuplicate = (pair == previousPair); @@ -450,34 +471,35 @@ void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) if (!isDuplicate) { //important to perform AABB check that is consistent with the broadphase - btDbvtProxy* pa=(btDbvtProxy*)pair.m_pProxy0; - btDbvtProxy* pb=(btDbvtProxy*)pair.m_pProxy1; - bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume); + btDbvtProxy* pa = (btDbvtProxy*)pair.m_pProxy0; + btDbvtProxy* pb = (btDbvtProxy*)pair.m_pProxy1; + bool hasOverlap = Intersect(pa->leaf->volume, pb->leaf->volume); if (hasOverlap) { needsRemoval = false; - } else + } + else { needsRemoval = true; } - } else + } + else { //remove duplicate needsRemoval = true; //should have no algorithm btAssert(!pair.m_algorithm); } - + if (needsRemoval) { - m_paircache->cleanOverlappingPair(pair,dispatcher); + m_paircache->cleanOverlappingPair(pair, dispatcher); pair.m_pProxy0 = 0; pair.m_pProxy1 = 0; invalidPair++; - } - + } } //perform a sort, to sort 'invalid' pairs to the end @@ -487,7 +509,7 @@ void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) } // -void btDbvtBroadphase::collide(btDispatcher* dispatcher) +void btDbvtBroadphase::collide(btDispatcher* dispatcher) { /*printf("---------------------------------------------------------\n"); printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); @@ -504,293 +526,303 @@ 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) + /* 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); + 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) + /* 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); + btDbvtTreeCollider collider(this); +#endif + 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); - ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); - current->leaf = m_sets[1].insert(curAabb,current); - current->stage = STAGECOUNT; - current = next; - } while(current); - m_fixedleft=m_sets[1].m_leaves; - m_needcleanup=true; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + curAabb = btDbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax); + current->leaf = m_sets[1].insert(curAabb, current); + current->stage = STAGECOUNT; + current = next; + } while (current); + m_fixedleft = m_sets[1].m_leaves; + m_needcleanup = true; } - /* collide dynamics */ + /* collide dynamics */ { - btDbvtTreeCollider collider(this); - if(m_deferedcollide) + btDbvtTreeCollider collider(this); + if (m_deferedcollide) { SPC(m_profiling.m_fdcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider); } - if(m_deferedcollide) + if (m_deferedcollide) { SPC(m_profiling.m_ddcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider); } } - /* clean up */ - if(m_needcleanup) + /* clean up */ + if (m_needcleanup) { SPC(m_profiling.m_cleanup); - btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); - if(pairs.size()>0) + btBroadphasePairArray& pairs = m_paircache->getOverlappingPairArray(); + if (pairs.size() > 0) { - - int ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100)); - for(int i=0;i<ni;++i) + int ni = btMin(pairs.size(), btMax<int>(m_newpairs, (pairs.size() * m_cupdates) / 100)); + for (int i = 0; i < ni; ++i) { - btBroadphasePair& p=pairs[(m_cid+i)%pairs.size()]; - btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; - btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; - if(!Intersect(pa->leaf->volume,pb->leaf->volume)) + btBroadphasePair& p = pairs[(m_cid + i) % pairs.size()]; + 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->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); + if (pa->m_uniqueId > pb->m_uniqueId) + btSwap(pa, pb); #endif - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; + m_paircache->removeOverlappingPair(pa, pb, dispatcher); + --ni; + --i; } } - if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + 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; } + 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; + { + m_updates_ratio = 0; + } + m_updates_done /= 2; + m_updates_call /= 2; } // -void btDbvtBroadphase::optimize() +void btDbvtBroadphase::optimize() { m_sets[0].optimizeTopDown(); m_sets[1].optimizeTopDown(); } // -btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() +btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() { - return(m_paircache); + return (m_paircache); } // -const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const +const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const { - return(m_paircache); + return (m_paircache); } // -void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const +void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const { + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds; - 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); + 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; + 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(); + bounds = btDbvtVolume::FromCR(btVector3(0, 0, 0), 0); + aabbMin = bounds.Mins(); + aabbMax = bounds.Maxs(); } void btDbvtBroadphase::resetPool(btDispatcher* dispatcher) { - int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; if (!totalObjects) { //reset internal dynamic tree data structures m_sets[0].clear(); m_sets[1].clear(); - - m_deferedcollide = false; - m_needcleanup = true; - 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_gid = 0; - m_pid = 0; - m_cid = 0; - for(int i=0;i<=STAGECOUNT;++i) + + m_deferedcollide = false; + m_needcleanup = true; + 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_gid = 0; + m_pid = 0; + m_cid = 0; + for (int i = 0; i <= STAGECOUNT; ++i) { - m_stageRoots[i]=0; + m_stageRoots[i] = 0; } } } // -void btDbvtBroadphase::printStats() -{} +void btDbvtBroadphase::printStats() +{ +} // #if DBVT_BP_ENABLE_BENCHMARK -struct btBroadphaseBenchmark +struct btBroadphaseBenchmark { - struct Experiment + struct Experiment { - const char* name; - int object_count; - int update_count; - int spawn_count; - int iterations; - btScalar speed; - btScalar amplitude; + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + btScalar speed; + btScalar amplitude; }; - struct Object + struct Object { - btVector3 center; - btVector3 extents; - btBroadphaseProxy* proxy; - btScalar time; - void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) + 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); + 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) + 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); + 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); + printf("%s : %u us (%u ms)\r\n", name, us, ms); } }; -void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) +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}, + 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) + }; + 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); + 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 */ + /* Create objects */ wallclock.reset(); objects.reserve(object_count); - for(int i=0;i<object_count;++i) + 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); + 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 */ + btBroadphaseBenchmark::OutputTime("\tInitialization", wallclock); + /* First update */ wallclock.reset(); - for(int i=0;i<objects.size();++i) + for (int i = 0; i < objects.size(); ++i) { - objects[i]->update(speed,amplitude,pbi); + objects[i]->update(speed, amplitude, pbi); } - btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); - /* Updates */ + btBroadphaseBenchmark::OutputTime("\tFirst update", wallclock); + /* Updates */ wallclock.reset(); - for(int i=0;i<experiment.iterations;++i) + for (int i = 0; i < experiment.iterations; ++i) { - for(int j=0;j<update_count;++j) - { - objects[j]->update(speed,amplitude,pbi); + 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 */ + btBroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations); + /* Clean up */ wallclock.reset(); - for(int i=0;i<objects.size();++i) + for (int i = 0; i < objects.size(); ++i) { - pbi->destroyProxy(objects[i]->proxy,0); + pbi->destroyProxy(objects[i]->proxy, 0); delete objects[i]; } objects.resize(0); - btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + btBroadphaseBenchmark::OutputTime("\tRelease", wallclock); } - } #else -void btDbvtBroadphase::benchmark(btBroadphaseInterface*) -{} +void btDbvtBroadphase::benchmark(btBroadphaseInterface*) +{ +} #endif #if DBVT_BP_PROFILE -#undef SPC +#undef SPC #endif - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 18b64ad0e57..a71feef53bb 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -24,15 +24,16 @@ subject to the following restrictions: // Compile time config // -#define DBVT_BP_PROFILE 0 +#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 +#define DBVT_BP_PREVENTFALSEUPDATE 0 +#define DBVT_BP_ACCURATESLEEPING 0 +#define DBVT_BP_ENABLE_BENCHMARK 0 +//#define DBVT_BP_MARGIN (btScalar)0.05 +extern btScalar gDbvtMargin; #if DBVT_BP_PROFILE -#define DBVT_BP_PROFILING_RATE 256 +#define DBVT_BP_PROFILING_RATE 256 #include "LinearMath/btQuickprof.h" #endif @@ -41,89 +42,90 @@ subject to the following restrictions: // struct btDbvtProxy : btBroadphaseProxy { - /* Fields */ + /* Fields */ //btDbvtAabbMm aabb; - btDbvtNode* leaf; - btDbvtProxy* links[2]; - int stage; - /* ctor */ - btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : - btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) + btDbvtNode* leaf; + btDbvtProxy* links[2]; + int stage; + /* ctor */ + btDbvtProxy(const btVector3& aabbMin, const btVector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : btBroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask) { - links[0]=links[1]=0; + links[0] = links[1] = 0; } }; -typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray; +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 +struct btDbvtBroadphase : btBroadphaseInterface { - /* Config */ - enum { - DYNAMIC_SET = 0, /* Dynamic set index */ - FIXED_SET = 1, /* Fixed set index */ - STAGECOUNT = 2 /* Number of stages */ + /* 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? + /* 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? + btAlignedObjectArray<btAlignedObjectArray<const btDbvtNode*> > m_rayTestStacks; #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; + 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); + /* 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); - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - virtual btOverlappingPairCache* getOverlappingPairCache(); - virtual const btOverlappingPairCache* getOverlappingPairCache() const; - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; - virtual void printStats(); + void collide(btDispatcher* dispatcher); + void optimize(); + /* btBroadphaseInterface Implementation */ + btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher); + virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher); + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + + virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + virtual btOverlappingPairCache* getOverlappingPairCache(); + virtual const btOverlappingPairCache* getOverlappingPairCache() const; + virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const; + virtual void printStats(); ///reset broadphase internal structures, to ensure determinism/reproducability virtual void resetPool(btDispatcher* dispatcher); - void performDeferredRemoval(btDispatcher* dispatcher); - - void setVelocityPrediction(btScalar prediction) + void performDeferredRemoval(btDispatcher* dispatcher); + + void setVelocityPrediction(btScalar prediction) { m_prediction = prediction; } @@ -132,15 +134,13 @@ struct btDbvtBroadphase : btBroadphaseInterface return m_prediction; } - ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. + ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. ///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase. ///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see ///http://code.google.com/p/bullet/issues/detail?id=223 - void setAabbForceUpdate( btBroadphaseProxy* absproxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* /*dispatcher*/); - - static void benchmark(btBroadphaseInterface*); - + void setAabbForceUpdate(btBroadphaseProxy* absproxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/); + static void benchmark(btBroadphaseInterface*); }; #endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp index 20768225b3a..d76d408aa65 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp @@ -17,6 +17,4 @@ subject to the following restrictions: btDispatcher::~btDispatcher() { - } - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 89c307d14ca..b09b7d4d422 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -20,7 +20,7 @@ subject to the following restrictions: class btCollisionAlgorithm; struct btBroadphaseProxy; class btRigidBody; -class btCollisionObject; +class btCollisionObject; class btOverlappingPairCache; struct btCollisionObjectWrapper; @@ -35,73 +35,76 @@ struct btDispatcherInfo DISPATCH_CONTINUOUS }; btDispatcherInfo() - :m_timeStep(btScalar(0.)), - m_stepCount(0), - m_dispatchFunc(DISPATCH_DISCRETE), - m_timeOfImpact(btScalar(1.)), - m_useContinuous(true), - m_debugDraw(0), - m_enableSatConvex(false), - m_enableSPU(true), - m_useEpa(true), - m_allowedCcdPenetration(btScalar(0.04)), - m_useConvexConservativeDistanceUtil(false), - m_convexConservativeDistanceThreshold(0.0f) + : m_timeStep(btScalar(0.)), + m_stepCount(0), + m_dispatchFunc(DISPATCH_DISCRETE), + m_timeOfImpact(btScalar(1.)), + m_useContinuous(true), + m_debugDraw(0), + m_enableSatConvex(false), + m_enableSPU(true), + m_useEpa(true), + m_allowedCcdPenetration(btScalar(0.04)), + m_useConvexConservativeDistanceUtil(false), + m_convexConservativeDistanceThreshold(0.0f), + m_deterministicOverlappingPairs(false) { - } - btScalar m_timeStep; - int m_stepCount; - int m_dispatchFunc; - mutable btScalar m_timeOfImpact; - bool m_useContinuous; - class btIDebugDraw* m_debugDraw; - bool m_enableSatConvex; - bool m_enableSPU; - bool m_useEpa; - btScalar m_allowedCcdPenetration; - bool m_useConvexConservativeDistanceUtil; - btScalar m_convexConservativeDistanceThreshold; + btScalar m_timeStep; + int m_stepCount; + int m_dispatchFunc; + mutable btScalar m_timeOfImpact; + bool m_useContinuous; + class btIDebugDraw* m_debugDraw; + bool m_enableSatConvex; + bool m_enableSPU; + bool m_useEpa; + btScalar m_allowedCcdPenetration; + bool m_useConvexConservativeDistanceUtil; + btScalar m_convexConservativeDistanceThreshold; + bool m_deterministicOverlappingPairs; +}; + +enum ebtDispatcherQueryType +{ + BT_CONTACT_POINT_ALGORITHMS = 1, + BT_CLOSEST_POINT_ALGORITHMS = 2 }; ///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 { - - public: - virtual ~btDispatcher() ; + virtual ~btDispatcher(); - virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0; + virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0; - virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0; + virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1) = 0; - virtual void releaseManifold(btPersistentManifold* manifold)=0; + virtual void releaseManifold(btPersistentManifold* manifold) = 0; - virtual void clearManifold(btPersistentManifold* manifold)=0; + virtual void clearManifold(btPersistentManifold* manifold) = 0; - virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) = 0; + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) = 0; - virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)=0; + virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) = 0; - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) =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 btPersistentManifold** getInternalManifoldPointer() = 0; - virtual btPoolAllocator* getInternalManifoldPool() = 0; + virtual btPoolAllocator* getInternalManifoldPool() = 0; - virtual const btPoolAllocator* getInternalManifoldPool() const = 0; + virtual const btPoolAllocator* getInternalManifoldPool() const = 0; - virtual void* allocateCollisionAlgorithm(int size) = 0; - - virtual void freeCollisionAlgorithm(void* ptr) = 0; + virtual void* allocateCollisionAlgorithm(int size) = 0; + virtual void freeCollisionAlgorithm(void* ptr) = 0; }; - -#endif //BT_DISPATCHER_H +#endif //BT_DISPATCHER_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp deleted file mode 100644 index 81369fe9b50..00000000000 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ /dev/null @@ -1,489 +0,0 @@ -/* -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(); -} - - - - - - -void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy); - aabbMin = multiProxy->m_aabbMin; - aabbMax = multiProxy->m_aabbMax; -} - -void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) -{ - for (int i=0;i<m_multiSapProxies.size();i++) - { - rayCallback.process(m_multiSapProxies[i]); - } -} - - -//#include <stdio.h> - -void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) -{ - btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy); - multiProxy->m_aabbMin = aabbMin; - multiProxy->m_aabbMax = aabbMax; - - -// bool fullyContained = false; -// bool alreadyInSimple = false; - - - - - struct MyNodeOverlapCallback : public btNodeOverlapCallback - { - btMultiSapBroadphase* m_multiSap; - btMultiSapProxy* m_multiProxy; - btDispatcher* m_dispatcher; - - MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) - :m_multiSap(multiSap), - m_multiProxy(multiProxy), - m_dispatcher(dispatcher) - { - - } - - virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) - { - btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; - - int containingBroadphaseIndex = -1; - //already found? - for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++) - { - - if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) - { - containingBroadphaseIndex = i; - break; - } - } - if (containingBroadphaseIndex<0) - { - //add it - btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy); - m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase); - - } - } - }; - - MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); - - - - - if (m_optimizedAabbTree) - m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - int i; - - for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) - { - btVector3 worldAabbMin,worldAabbMax; - multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); - bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - if (!overlapsBroadphase) - { - //remove it now - btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; - - btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; - bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); - - multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); - multiProxy->m_bridgeProxies.pop_back(); - - } - } - - - /* - - if (1) - { - - //find broadphase that contain this multiProxy - int numChildBroadphases = getBroadphaseArray().size(); - for (int i=0;i<numChildBroadphases;i++) - { - btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; - btVector3 worldAabbMin,worldAabbMax; - childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); - bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - - // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - int containingBroadphaseIndex = -1; - - //if already contains this - - for (int i=0;i<multiProxy->m_bridgeProxies.size();i++) - { - if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) - { - containingBroadphaseIndex = i; - } - alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); - } - - if (overlapsBroadphase) - { - if (containingBroadphaseIndex<0) - { - btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,childBroadphase); - } - } else - { - if (containingBroadphaseIndex>=0) - { - //remove - btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; - - btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; - bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); - - multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); - multiProxy->m_bridgeProxies.pop_back(); - } - } - } - - - ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) - ///hopefully we don't end up with many entries here (can assert/provide feedback on stats) - if (0)//!multiProxy->m_bridgeProxies.size()) - { - ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision - ///this is needed to be able to calculate the aabb overlap - btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); - } - } - - if (!multiProxy->m_bridgeProxies.size()) - { - ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision - ///this is needed to be able to calculate the aabb overlap - btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); - } -*/ - - - //update - for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) - { - btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; - bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); - } - -} -bool stopUpdating=false; - - - -class btMultiSapBroadphasePairSortPredicate -{ - public: - - bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) const - { - btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0; - - return aProxy0 > bProxy0 || - (aProxy0 == bProxy0 && aProxy1 > bProxy1) || - (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); - } -}; - - - ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb -void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - -// m_simpleBroadphase->calculateOverlappingPairs(dispatcher); - - if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); - - // quicksort(overlappingPairArray,0,overlappingPairArray.size()); - - overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - - - int i; - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;i<overlappingPairArray.size();i++) - { - - btBroadphasePair& pair = overlappingPairArray[i]; - - btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0; - btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; - - bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); - - previousPair = pair; - - bool needsRemoval = false; - - if (!isDuplicate) - { - bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); - - if (hasOverlap) - { - needsRemoval = false;//callback->processOverlap(pair); - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); - - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - m_invalidPair++; - gOverlappingPairs--; - } - - } - - ///if you don't like to skip the invalid pairs in the array, execute following code: - #define CLEAN_INVALID_PAIRS 1 - #ifdef CLEAN_INVALID_PAIRS - - //perform a sort, to sort 'invalid' pairs to the end - //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); - overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS - - //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); - } - - -} - - -bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) -{ - btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; - btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; - - return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, - multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); - -} - - -void btMultiSapBroadphase::printStats() -{ -/* printf("---------------------------------\n"); - - printf("btMultiSapBroadphase.h\n"); - printf("numHandles = %d\n",m_multiSapProxies.size()); - //find broadphase that contain this multiProxy - int numChildBroadphases = getBroadphaseArray().size(); - for (int i=0;i<numChildBroadphases;i++) - { - - btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; - childBroadphase->printStats(); - - } - */ - -} - -void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) -{ - // not yet -} diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h deleted file mode 100644 index 7bcfe6b132a..00000000000 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ /dev/null @@ -1,151 +0,0 @@ -/* -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 research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead. -///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(aabbMin,aabbMax,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); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); - - 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(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); - aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); - } - - void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); - - virtual void printStats(); - - void quicksort (btBroadphasePairArray& a, int lo, int hi); - - ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher); - -}; - -#endif //BT_MULTI_SAP_BROADPHASE diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index ad69fcbd712..8ce1087c9f1 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btOverlappingPairCache.h" #include "btDispatcher.h" @@ -23,127 +21,95 @@ subject to the following restrictions: #include <stdio.h> -int gOverlappingPairs = 0; - -int gRemovePairs =0; -int gAddedPairs =0; -int gFindPairs =0; - - - - -btHashedOverlappingPairCache::btHashedOverlappingPairCache(): - m_overlapFilterCallback(0), - m_ghostPairCallback(0) +btHashedOverlappingPairCache::btHashedOverlappingPairCache() : m_overlapFilterCallback(0), + m_ghostPairCallback(0) { - int initialAllocatedSize= 2; + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - - btHashedOverlappingPairCache::~btHashedOverlappingPairCache() { } - - -void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) { if (pair.m_algorithm && dispatcher) { { pair.m_algorithm->~btCollisionAlgorithm(); dispatcher->freeCollisionAlgorithm(pair.m_algorithm); - pair.m_algorithm=0; + pair.m_algorithm = 0; } } } - - - -void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class CleanPairCallback : public btOverlapCallback + class CleanPairCallback : public btOverlapCallback { btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; + btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) + CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) + : m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { if ((pair.m_pProxy0 == m_cleanProxy) || (pair.m_pProxy1 == m_cleanProxy)) { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + m_pairCache->cleanOverlappingPair(pair, m_dispatcher); } return false; } - }; - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); + CleanPairCallback cleanPairs(proxy, this, dispatcher); + processAllOverlappingPairs(&cleanPairs, dispatcher); } - - - -void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class RemovePairCallback : public btOverlapCallback + class RemovePairCallback : public btOverlapCallback { btBroadphaseProxy* m_obsoleteProxy; public: RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) + : m_obsoleteProxy(obsoleteProxy) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); + (pair.m_pProxy1 == m_obsoleteProxy)); } - }; - RemovePairCallback removeCallback(proxy); - processAllOverlappingPairs(&removeCallback,dispatcher); + processAllOverlappingPairs(&removeCallback, dispatcher); } - - - - btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - gFindPairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); + if (proxy0->m_uniqueId > proxy1->m_uniqueId) + btSwap(proxy0, proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - /*if (proxyId1 > proxyId2) + /*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)); + 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()) { @@ -168,9 +134,8 @@ btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* prox //#include <stdio.h> -void btHashedOverlappingPairCache::growTables() +void btHashedOverlappingPairCache::growTables() { - int newCapacity = m_overlappingPairArray.capacity(); if (m_hashTable.size() < newCapacity) @@ -181,10 +146,9 @@ void btHashedOverlappingPairCache::growTables() m_hashTable.resize(newCapacity); m_next.resize(newCapacity); - int i; - for (i= 0; i < newCapacity; ++i) + for (i = 0; i < newCapacity; ++i) { m_hashTable[i] = BT_NULL_PAIR; } @@ -193,35 +157,31 @@ void btHashedOverlappingPairCache::growTables() m_next[i] = BT_NULL_PAIR; } - for(i=0;i<curHashtableSize;i++) + 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 + 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->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); + if (proxy0->m_uniqueId > proxy1->m_uniqueId) + 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 - + 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) @@ -243,7 +203,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx //this is where we add an actual pair, so also call the 'ghost' if (m_ghostPairCallback) - m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); + m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); int newCapacity = m_overlappingPairArray.capacity(); @@ -251,15 +211,14 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx { growTables(); //hash with new capacity - hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + 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 = new (mem) btBroadphasePair(*proxy0, *proxy1); + // pair->m_pProxy0 = proxy0; + // pair->m_pProxy1 = proxy1; pair->m_algorithm = 0; pair->m_internalTmpValue = 0; - m_next[count] = m_hashTable[hash]; m_hashTable[hash] = count; @@ -267,20 +226,17 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx return pair; } - - -void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) +void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) { - gRemovePairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); + if (proxy0->m_uniqueId > proxy1->m_uniqueId) + btSwap(proxy0, proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - /*if (proxyId1 > proxyId2) + /*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)); + 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) @@ -288,7 +244,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return 0; } - cleanOverlappingPair(*pair,dispatcher); + cleanOverlappingPair(*pair, dispatcher); void* userData = pair->m_internalInfo1; @@ -326,7 +282,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro int lastPairIndex = m_overlappingPairArray.size() - 1; if (m_ghostPairCallback) - m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher); // If the removed pair is the last pair, we are done. if (lastPairIndex == pairIndex) @@ -337,8 +293,8 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro // 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)); + /* 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); @@ -372,44 +328,106 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return userData; } //#include <stdio.h> - -void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +#include "LinearMath/btQuickprof.h" +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher) { - + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); int i; -// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); - for (i=0;i<m_overlappingPairArray.size();) + // 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 + removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher); + } + else { i++; } } } -void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +struct MyPairIndex +{ + int m_orgIndex; + int m_uidA0; + int m_uidA1; +}; + +class MyPairIndeSortPredicate +{ +public: + bool operator()(const MyPairIndex& a, const MyPairIndex& b) const + { + const int uidA0 = a.m_uidA0; + const int uidB0 = b.m_uidA0; + const int uidA1 = a.m_uidA1; + const int uidB1 = b.m_uidA1; + return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); + } +}; + +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo) +{ + if (dispatchInfo.m_deterministicOverlappingPairs) + { + btBroadphasePairArray& pa = getOverlappingPairArray(); + btAlignedObjectArray<MyPairIndex> indices; + { + BT_PROFILE("sortOverlappingPairs"); + indices.resize(pa.size()); + for (int i = 0; i < indices.size(); i++) + { + const btBroadphasePair& p = pa[i]; + const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1; + const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1; + + indices[i].m_uidA0 = uidA0; + indices[i].m_uidA1 = uidA1; + indices[i].m_orgIndex = i; + } + indices.quickSort(MyPairIndeSortPredicate()); + } + { + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); + int i; + for (i = 0; i < indices.size();) + { + btBroadphasePair* pair = &pa[indices[i].m_orgIndex]; + if (callback->processOverlap(*pair)) + { + removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher); + } + else + { + i++; + } + } + } + } + else + { + processAllOverlappingPairs(callback, dispatcher); + } +} + +void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) { ///need to keep hashmap in sync with pair address, so rebuild all btBroadphasePairArray tmpPairs; int i; - for (i=0;i<m_overlappingPairArray.size();i++) + for (i = 0; i < m_overlappingPairArray.size(); i++) { tmpPairs.push_back(m_overlappingPairArray[i]); } - for (i=0;i<tmpPairs.size();i++) + for (i = 0; i < tmpPairs.size(); i++) { - removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher); + removeOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher); } - + for (i = 0; i < m_next.size(); i++) { m_next[i] = BT_NULL_PAIR; @@ -417,32 +435,28 @@ void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher tmpPairs.quickSort(btBroadphasePairSortPredicate()); - for (i=0;i<tmpPairs.size();i++) + for (i = 0; i < tmpPairs.size(); i++) { - addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1); + addOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1); } - - } - -void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher ) +void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) { if (!hasDeferredRemoval()) { - btBroadphasePair findPair(*proxy0,*proxy1); + btBroadphasePair findPair(*proxy0, *proxy1); int findIndex = m_overlappingPairArray.findLinearSearch(findPair); if (findIndex < m_overlappingPairArray.size()) { - gOverlappingPairs--; btBroadphasePair& pair = m_overlappingPairArray[findIndex]; void* userData = pair.m_internalInfo1; - cleanOverlappingPair(pair,dispatcher); + cleanOverlappingPair(pair, dispatcher); if (m_ghostPairCallback) - m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); - - m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher); + + m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1); m_overlappingPairArray.pop_back(); return userData; } @@ -451,99 +465,73 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return 0; } - - - - - - - -btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { //don't add overlap with own btAssert(proxy0 != proxy1); - if (!needsBroadphaseCollision(proxy0,proxy1)) + if (!needsBroadphaseCollision(proxy0, proxy1)) return 0; - + void* mem = &m_overlappingPairArray.expandNonInitializing(); - btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); - - gOverlappingPairs++; - gAddedPairs++; - + btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0, *proxy1); + if (m_ghostPairCallback) m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); return pair; - } ///this findPair becomes really slow. Either sort the list to speedup the query, or ///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* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - if (!needsBroadphaseCollision(proxy0,proxy1)) + if (!needsBroadphaseCollision(proxy0, proxy1)) return 0; - btBroadphasePair tmpPair(*proxy0,*proxy1); + btBroadphasePair tmpPair(*proxy0, *proxy1); int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); if (findIndex < m_overlappingPairArray.size()) { //btAssert(it != m_overlappingPairSet.end()); - btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; + btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; return pair; } return 0; } - - - - - - - - - //#include <stdio.h> -void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher) { - int i; - for (i=0;i<m_overlappingPairArray.size();) + for (i = 0; i < m_overlappingPairArray.size();) { - btBroadphasePair* pair = &m_overlappingPairArray[i]; if (callback->processOverlap(*pair)) { - cleanOverlappingPair(*pair,dispatcher); + cleanOverlappingPair(*pair, dispatcher); pair->m_pProxy0 = 0; pair->m_pProxy1 = 0; - m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1); m_overlappingPairArray.pop_back(); - gOverlappingPairs--; - } else + } + else { i++; } } } - - - -btSortedOverlappingPairCache::btSortedOverlappingPairCache(): - m_blockedForChanges(false), - m_hasDeferredRemoval(true), - m_overlapFilterCallback(0), - m_ghostPairCallback(0) +btSortedOverlappingPairCache::btSortedOverlappingPairCache() : m_blockedForChanges(false), + m_hasDeferredRemoval(true), + m_overlapFilterCallback(0), + m_ghostPairCallback(0) { - int initialAllocatedSize= 2; + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); } @@ -551,82 +539,73 @@ btSortedOverlappingPairCache::~btSortedOverlappingPairCache() { } -void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +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--; + pair.m_algorithm = 0; } } } - -void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class CleanPairCallback : public btOverlapCallback + class CleanPairCallback : public btOverlapCallback { btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; + btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) + CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) + : m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { if ((pair.m_pProxy0 == m_cleanProxy) || (pair.m_pProxy1 == m_cleanProxy)) { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + m_pairCache->cleanOverlappingPair(pair, m_dispatcher); } return false; } - }; - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); + CleanPairCallback cleanPairs(proxy, this, dispatcher); + processAllOverlappingPairs(&cleanPairs, dispatcher); } - -void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class RemovePairCallback : public btOverlapCallback + class RemovePairCallback : public btOverlapCallback { btBroadphaseProxy* m_obsoleteProxy; public: RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) + : m_obsoleteProxy(obsoleteProxy) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); + (pair.m_pProxy1 == m_obsoleteProxy)); } - }; RemovePairCallback removeCallback(proxy); - processAllOverlappingPairs(&removeCallback,dispatcher); + processAllOverlappingPairs(&removeCallback, dispatcher); } -void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) { //should already be sorted } - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 14614270476..56011899cb5 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_OVERLAPPING_PAIR_CACHE_H #define BT_OVERLAPPING_PAIR_CACHE_H - #include "btBroadphaseInterface.h" #include "btBroadphaseProxy.h" #include "btOverlappingPairCallback.h" @@ -24,174 +23,164 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" class btDispatcher; -typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray; +typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray; -struct btOverlapCallback +struct btOverlapCallback { virtual ~btOverlapCallback() - {} + { + } //return true for deletion of the pair - virtual bool processOverlap(btBroadphasePair& pair) = 0; - + virtual bool processOverlap(btBroadphasePair& pair) = 0; }; struct btOverlapFilterCallback { virtual ~btOverlapFilterCallback() - {} + { + } // return true when pairs need collision - virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0; + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const = 0; }; - - - - - - -extern int gRemovePairs; -extern int gAddedPairs; -extern int gFindPairs; - -const int BT_NULL_PAIR=0xffffffff; +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 ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor - virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; - - virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; + virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; - virtual btBroadphasePairArray& getOverlappingPairArray() = 0; + virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; - virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0; + virtual btBroadphasePairArray& getOverlappingPairArray() = 0; - virtual int getNumOverlappingPairs() const = 0; + virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0; - virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0; + virtual int getNumOverlappingPairs() const = 0; + virtual bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const = 0; + virtual btOverlapFilterCallback* getOverlapFilterCallback() = 0; + virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0; - virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; + virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher) = 0; + virtual void processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& /*dispatchInfo*/) + { + processAllOverlappingPairs(callback, dispatcher); + } virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; - virtual bool hasDeferredRemoval() = 0; - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0; - - virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0; + virtual bool hasDeferredRemoval() = 0; + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) = 0; + virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 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 + +ATTRIBUTE_ALIGNED16(class) +btHashedOverlappingPairCache : public btOverlappingPairCache { - btBroadphasePairArray m_overlappingPairArray; + btBroadphasePairArray m_overlappingPairArray; btOverlapFilterCallback* m_overlapFilterCallback; protected: - - btAlignedObjectArray<int> m_hashTable; - btAlignedObjectArray<int> m_next; - btOverlappingPairCallback* m_ghostPairCallback; - + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; + btOverlappingPairCallback* m_ghostPairCallback; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btHashedOverlappingPairCache(); virtual ~btHashedOverlappingPairCache(); - - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + void removeOverlappingPairsContainingProxy(btBroadphaseProxy * proxy, btDispatcher * dispatcher); + + virtual void* removeOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1, btDispatcher * dispatcher); - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); - - SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const { if (m_overlapFilterCallback) - return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + 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) + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) { - gAddedPairs++; - - if (!needsBroadphaseCollision(proxy0,proxy1)) + if (!needsBroadphaseCollision(proxy0, proxy1)) return 0; - return internalAddPair(proxy0,proxy1); + return internalAddPair(proxy0, proxy1); } - + void cleanProxyFromPairs(btBroadphaseProxy * proxy, btDispatcher * dispatcher); - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher * dispatcher); - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback * callback, btDispatcher * dispatcher, const struct btDispatcherInfo& dispatchInfo); - virtual btBroadphasePair* getOverlappingPairArrayPtr() + virtual btBroadphasePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; } - const btBroadphasePair* getOverlappingPairArrayPtr() const + const btBroadphasePair* getOverlappingPairArrayPtr() const { return &m_overlappingPairArray[0]; } - btBroadphasePairArray& getOverlappingPairArray() + btBroadphasePairArray& getOverlappingPairArray() { return m_overlappingPairArray; } - const btBroadphasePairArray& getOverlappingPairArray() const + const btBroadphasePairArray& getOverlappingPairArray() const { return m_overlappingPairArray; } - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); + void cleanOverlappingPair(btBroadphasePair & pair, btDispatcher * dispatcher); - - - btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); + btBroadphasePair* findPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1); int GetCount() const { return m_overlappingPairArray.size(); } -// btBroadphasePair* GetPairs() { return m_pairs; } + // btBroadphasePair* GetPairs() { return m_pairs; } btOverlapFilterCallback* getOverlapFilterCallback() { return m_overlapFilterCallback; } - void setOverlapFilterCallback(btOverlapFilterCallback* callback) + void setOverlapFilterCallback(btOverlapFilterCallback * callback) { m_overlapFilterCallback = callback; } - int getNumOverlappingPairs() const + int getNumOverlappingPairs() const { return m_overlappingPairArray.size(); } + private: - - btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + btBroadphasePair* internalAddPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1); - void growTables(); + 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; } @@ -211,43 +200,37 @@ private: } */ - - - SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) + SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) { - int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + unsigned int key = proxyId1 | (proxyId2 << 16); // Thomas Wang's hash key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); key += ~(key << 11); - key ^= (key >> 16); - return static_cast<unsigned int>(key); + key ^= (key >> 16); + return key; } - - - - - SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) + 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 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2); - #endif +#endif int index = m_hashTable[hash]; - - while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + + while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) { index = m_next[index]; } - if ( index == BT_NULL_PAIR ) + if (index == BT_NULL_PAIR) { return NULL; } @@ -257,155 +240,136 @@ private: return &m_overlappingPairArray[index]; } - virtual bool hasDeferredRemoval() + virtual bool hasDeferredRemoval() { return false; } - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + virtual void setInternalGhostPairCallback(btOverlappingPairCallback * ghostPairCallback) { m_ghostPairCallback = ghostPairCallback; } - virtual void sortOverlappingPairs(btDispatcher* dispatcher); - - - + virtual void sortOverlappingPairs(btDispatcher * dispatcher); }; - - - ///btSortedOverlappingPairCache maintains the objects with overlapping AABB ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase -class btSortedOverlappingPairCache : public btOverlappingPairCache +class btSortedOverlappingPairCache : public btOverlappingPairCache { - protected: - //avoid brute-force finding all the time - btBroadphasePairArray m_overlappingPairArray; +protected: + //avoid brute-force finding all the time + btBroadphasePairArray m_overlappingPairArray; - //during the dispatch, check that user doesn't destroy/create proxy - bool m_blockedForChanges; + //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; + ///by default, do the removal during the pair traversal + bool m_hasDeferredRemoval; - btOverlappingPairCallback* m_ghostPairCallback; + //if set, use the callback instead of the built in filter in needBroadphaseCollision + btOverlapFilterCallback* m_overlapFilterCallback; - public: - - btSortedOverlappingPairCache(); - virtual ~btSortedOverlappingPairCache(); + btOverlappingPairCallback* m_ghostPairCallback; - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); +public: + btSortedOverlappingPairCache(); + virtual ~btSortedOverlappingPairCache(); - void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher); - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - - btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher); - btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher); - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); + btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); - inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const - { - if (m_overlapFilterCallback) - return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher); - bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; - collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - - return collides; - } - - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher); - const btBroadphasePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } + 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; + } - btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } - - btOverlapFilterCallback* getOverlapFilterCallback() - { - return m_overlapFilterCallback; - } + btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } - void setOverlapFilterCallback(btOverlapFilterCallback* callback) - { - m_overlapFilterCallback = callback; - } + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } - virtual bool hasDeferredRemoval() - { - return m_hasDeferredRemoval; - } + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) - { - m_ghostPairCallback = ghostPairCallback; - } + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } - virtual void sortOverlappingPairs(btDispatcher* dispatcher); - + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } -}; + virtual bool hasDeferredRemoval() + { + return m_hasDeferredRemoval; + } + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + virtual void sortOverlappingPairs(btDispatcher* dispatcher); +}; ///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. class btNullPairCache : public btOverlappingPairCache { - - btBroadphasePairArray m_overlappingPairArray; + btBroadphasePairArray m_overlappingPairArray; public: - - virtual btBroadphasePair* getOverlappingPairArrayPtr() + virtual btBroadphasePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; } - const btBroadphasePair* getOverlappingPairArrayPtr() const + const btBroadphasePair* getOverlappingPairArrayPtr() const { return &m_overlappingPairArray[0]; } - btBroadphasePairArray& getOverlappingPairArray() + btBroadphasePairArray& getOverlappingPairArray() { return m_overlappingPairArray; } - - virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) - { + virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/, btDispatcher* /*dispatcher*/) + { } virtual int getNumOverlappingPairs() const @@ -413,16 +377,23 @@ public: return 0; } - virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) + virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/, btDispatcher* /*dispatcher*/) { - } - virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) + bool needsBroadphaseCollision(btBroadphaseProxy*, btBroadphaseProxy*) const + { + return true; + } + btOverlapFilterCallback* getOverlapFilterCallback() + { + return 0; + } + virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) { } - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) + virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* /*dispatcher*/) { } @@ -431,39 +402,33 @@ public: return 0; } - virtual bool hasDeferredRemoval() + virtual bool hasDeferredRemoval() { return true; } - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) { - } - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/) { return 0; } - virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/) + virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/, btDispatcher* /*dispatcher*/) { return 0; } - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/) { } - - virtual void sortOverlappingPairs(btDispatcher* dispatcher) + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) { - (void) dispatcher; + (void)dispatcher; } - - }; - -#endif //BT_OVERLAPPING_PAIR_CACHE_H - - +#endif //BT_OVERLAPPING_PAIR_CACHE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h index 9c7b6f81367..d16c72542f1 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h @@ -18,23 +18,24 @@ subject to the following restrictions: #define OVERLAPPING_PAIR_CALLBACK_H class btDispatcher; -struct btBroadphasePair; +struct btBroadphasePair; ///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. class btOverlappingPairCallback { +protected: + btOverlappingPairCallback() {} + public: virtual ~btOverlappingPairCallback() { - } - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0; - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0; + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0; + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) = 0; + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0, btDispatcher* dispatcher) = 0; }; -#endif //OVERLAPPING_PAIR_CALLBACK_H +#endif //OVERLAPPING_PAIR_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 889216df509..19f1737b73c 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -21,43 +21,38 @@ subject to the following restrictions: #define RAYAABB2 -btQuantizedBvh::btQuantizedBvh() : - m_bulletVersion(BT_BULLET_VERSION), - m_useQuantization(false), - //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) - m_traversalMode(TRAVERSAL_STACKLESS) - //m_traversalMode(TRAVERSAL_RECURSIVE) - ,m_subtreeHeaderCount(0) //PCK: add this line +btQuantizedBvh::btQuantizedBvh() : m_bulletVersion(BT_BULLET_VERSION), + m_useQuantization(false), + //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) + m_traversalMode(TRAVERSAL_STACKLESS) + //m_traversalMode(TRAVERSAL_RECURSIVE) + , + m_subtreeHeaderCount(0) //PCK: add this line { - m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY); - m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + m_bvhAabbMin.setValue(-SIMD_INFINITY, -SIMD_INFINITY, -SIMD_INFINITY); + m_bvhAabbMax.setValue(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); } - - - - 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_quantizedContiguousNodes.resize(2 * numLeafNodes); } m_curNodeIndex = 0; - buildTree(0,numLeafNodes); + 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()) + if (m_useQuantization && !m_SubtreeHeaders.size()) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); @@ -73,29 +68,24 @@ void btQuantizedBvh::buildInternal() m_leafNodes.clear(); } - - ///just for debugging, to visualize the individual patches/subtrees #ifdef DEBUG_PATCH_COLORS -btVector3 color[4]= -{ - btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1), - btVector3(0,1,1) -}; -#endif //DEBUG_PATCH_COLORS - - +btVector3 color[4] = + { + btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1), + btVector3(0, 1, 1)}; +#endif //DEBUG_PATCH_COLORS -void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin) +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); + 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_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize; m_useQuantization = true; @@ -103,23 +93,22 @@ void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btV unsigned short vecIn[3]; btVector3 v; { - quantize(vecIn,m_bvhAabbMin,false); + quantize(vecIn, m_bvhAabbMin, false); v = unQuantize(vecIn); - m_bvhAabbMin.setMin(v-clampValue); + m_bvhAabbMin.setMin(v - clampValue); } + aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize; { - quantize(vecIn,m_bvhAabbMax,true); + quantize(vecIn, m_bvhAabbMax, true); v = unQuantize(vecIn); - m_bvhAabbMax.setMax(v+clampValue); + m_bvhAabbMax.setMax(v + clampValue); } aabbSize = m_bvhAabbMax - m_bvhAabbMin; - m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; + m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize; } } - - - btQuantizedBvh::~btQuantizedBvh() { } @@ -127,104 +116,100 @@ btQuantizedBvh::~btQuantizedBvh() #ifdef DEBUG_TREE_BUILDING int gStackDepth = 0; int gMaxStackDepth = 0; -#endif //DEBUG_TREE_BUILDING +#endif //DEBUG_TREE_BUILDING -void btQuantizedBvh::buildTree (int startIndex,int endIndex) +void btQuantizedBvh::buildTree(int startIndex, int endIndex) { #ifdef DEBUG_TREE_BUILDING gStackDepth++; if (gStackDepth > gMaxStackDepth) gMaxStackDepth = gStackDepth; -#endif //DEBUG_TREE_BUILDING - +#endif //DEBUG_TREE_BUILDING int splitAxis, splitIndex, i; - int numIndices =endIndex-startIndex; + int numIndices = endIndex - startIndex; int curIndex = m_curNodeIndex; - btAssert(numIndices>0); + btAssert(numIndices > 0); - if (numIndices==1) + if (numIndices == 1) { #ifdef DEBUG_TREE_BUILDING gStackDepth--; -#endif //DEBUG_TREE_BUILDING - - assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); +#endif //DEBUG_TREE_BUILDING + + assignInternalNodeFromLeafNode(m_curNodeIndex, startIndex); m_curNodeIndex++; - return; + 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); + splitAxis = calcSplittingAxis(startIndex, endIndex); + + splitIndex = sortAndCalcSplittingIndex(startIndex, endIndex, splitAxis); int internalNodeIndex = m_curNodeIndex; - + //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value. //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values - setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization - setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization - - - for (i=startIndex;i<endIndex;i++) + setInternalNodeAabbMin(m_curNodeIndex, m_bvhAabbMax); //can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization + setInternalNodeAabbMax(m_curNodeIndex, m_bvhAabbMin); //can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization + + for (i = startIndex; i < endIndex; i++) { - mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i)); + mergeInternalNodeAabb(m_curNodeIndex, getAabbMin(i), getAabbMax(i)); } m_curNodeIndex++; - //internalNode->m_escapeIndex; - + int leftChildNodexIndex = m_curNodeIndex; //build left child tree - buildTree(startIndex,splitIndex); + buildTree(startIndex, splitIndex); int rightChildNodexIndex = m_curNodeIndex; //build right child tree - buildTree(splitIndex,endIndex); + buildTree(splitIndex, endIndex); #ifdef DEBUG_TREE_BUILDING gStackDepth--; -#endif //DEBUG_TREE_BUILDING +#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 sizeQuantizedNode = sizeof(btQuantizedBvhNode); const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) { - updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); + updateSubtreeHeaders(leftChildNodexIndex, rightChildNodexIndex); } - } else + } + else { - } - setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); - + setInternalNodeEscapeIndex(internalNodeIndex, escapeIndex); } -void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +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)); - + 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)); + int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode)); - if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + if (leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(leftChildNode); @@ -232,7 +217,7 @@ void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChild subtree.m_subtreeSize = leftSubTreeSize; } - if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + if (rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(rightChildNode); @@ -244,32 +229,31 @@ void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChild m_subtreeHeaderCount = m_SubtreeHeaders.size(); } - -int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) +int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis) { int i; - int splitIndex =startIndex; + int splitIndex = startIndex; int numIndices = endIndex - startIndex; btScalar splitValue; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i<endIndex;i++) + 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; + btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i)); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); - + means *= (btScalar(1.) / (btScalar)numIndices); + splitValue = means[splitAxis]; - + //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); + btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i)); if (center[splitAxis] > splitValue) { //swap - swapLeafNodes(i,splitIndex); + swapLeafNodes(i, splitIndex); splitIndex++; } } @@ -279,56 +263,53 @@ int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int sp //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) + //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))); - + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); + if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + bool unbal = (splitIndex == startIndex) || (splitIndex == (endIndex)); (void)unbal; btAssert(!unbal); return splitIndex; } - -int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) +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; + 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++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); - means+=center; + btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i)); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); - - for (i=startIndex;i<endIndex;i++) + means *= (btScalar(1.) / (btScalar)numIndices); + + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i)); - btVector3 diff2 = center-means; + btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i)); + btVector3 diff2 = center - means; diff2 = diff2 * diff2; variance += diff2; } - variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); - + variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); + return variance.maxAxis(); } - - -void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const { //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) @@ -337,38 +318,35 @@ void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb ///quantize query AABB unsigned short int quantizedQueryAabbMin[3]; unsigned short int quantizedQueryAabbMax[3]; - quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0); - quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1); + 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: + 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); + walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); } break; - default: - //unsupported - btAssert(0); + default: + //unsupported + btAssert(0); } - } else + } + else { - walkStacklessTree(nodeCallback,aabbMin,aabbMax); + walkStacklessTree(nodeCallback, aabbMin, aabbMax); } } - -int maxIterations = 0; - - -void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const { btAssert(!m_useQuantization); @@ -382,33 +360,31 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const while (curIndex < m_curNodeIndex) { //catch bugs in tree data - btAssert (walkIterations < m_curNodeIndex); + btAssert(walkIterations < m_curNodeIndex); walkIterations++; - aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); + 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); - } - + nodeCallback->processNode(rootNode->m_subPart, rootNode->m_triangleIndex); + } + //PCK: unsigned instead of bool if ((aabbOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + else { escapeIndex = rootNode->m_escapeIndex; rootNode += escapeIndex; curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - } /* @@ -432,39 +408,38 @@ void btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback } */ -void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +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); + 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 + nodeCallback->processNode(currentNode->getPartId(), currentNode->getTriangleIndex()); + } + else { //process left and right children - const btQuantizedBvhNode* leftChildNode = currentNode+1; - walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + 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); + const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode + 1 : leftChildNode + leftChildNode->getEscapeIndex(); + walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); } - } + } } - - -void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const { btAssert(!m_useQuantization); @@ -473,11 +448,11 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall int walkIterations = 0; bool isLeafNode; //PCK: unsigned instead of bool - unsigned aabbOverlap=0; - unsigned rayBoxOverlap=0; + unsigned aabbOverlap = 0; + unsigned rayBoxOverlap = 0; btScalar lambda_max = 1.0; - - /* Quick pruning by quantized box */ + + /* Quick pruning by quantized box */ btVector3 rayAabbMin = raySource; btVector3 rayAabbMax = raySource; rayAabbMin.setMin(rayTarget); @@ -488,15 +463,15 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall rayAabbMax += aabbMax; #ifdef RAYAABB2 - btVector3 rayDir = (rayTarget-raySource); - rayDir.normalize (); - lambda_max = rayDir.dot(rayTarget-raySource); + btVector3 rayDir = (rayTarget - raySource); + rayDir.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17 + lambda_max = rayDir.dot(rayTarget - raySource); ///what about division by zero? --> just set rayDirection[i] to 1.0 btVector3 rayDirectionInverse; rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + unsigned int sign[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; #endif btVector3 bounds[2]; @@ -505,7 +480,7 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall { btScalar param = 1.0; //catch bugs in tree data - btAssert (walkIterations < m_curNodeIndex); + btAssert(walkIterations < m_curNodeIndex); walkIterations++; @@ -515,51 +490,47 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall bounds[0] -= aabbMax; bounds[1] -= aabbMin; - aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); + aabbOverlap = TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, rootNode->m_aabbMinOrg, rootNode->m_aabbMaxOrg); //perhaps profile if it is worth doing the aabbOverlap test first #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 = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; + ///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 = aabbOverlap ? btRayAabb2(raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; #else btVector3 normal; - rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal); + rayBoxOverlap = btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); #endif isLeafNode = rootNode->m_escapeIndex == -1; - + //PCK: unsigned instead of bool if (isLeafNode && (rayBoxOverlap != 0)) { - nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); - } - + nodeCallback->processNode(rootNode->m_subPart, rootNode->m_triangleIndex); + } + //PCK: unsigned instead of bool if ((rayBoxOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + else { escapeIndex = rootNode->m_escapeIndex; rootNode += escapeIndex; curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - } - - -void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +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; @@ -567,7 +538,7 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; int escapeIndex; - + bool isLeafNode; //PCK: unsigned instead of bool unsigned boxBoxOverlap = 0; @@ -576,14 +547,14 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* btScalar lambda_max = 1.0; #ifdef RAYAABB2 - btVector3 rayDirection = (rayTarget-raySource); - rayDirection.normalize (); - lambda_max = rayDirection.dot(rayTarget-raySource); + btVector3 rayDirection = (rayTarget - raySource); + rayDirection.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17 + 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(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0]; rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1]; rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[2]; - unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; + unsigned int sign[3] = {rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; #endif /* Quick pruning by quantized box */ @@ -598,37 +569,36 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* unsigned short int quantizedQueryAabbMin[3]; unsigned short int quantizedQueryAabbMax[3]; - quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0); - quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1); + 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) + if (curIndex == drawPatch) { - btVector3 aabbMin,aabbMax; + btVector3 aabbMin, aabbMax; aabbMin = unQuantize(rootNode->m_quantizedAabbMin); aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + btVector3 color(1, 0, 0); + debugDrawerPtr->drawAabb(aabbMin, aabbMax, color); } -#endif//VISUALLY_ANALYZE_BVH +#endif //VISUALLY_ANALYZE_BVH //catch bugs in tree data - btAssert (walkIterations < subTreeSize); + btAssert(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); + boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode->m_quantizedAabbMin, rootNode->m_quantizedAabbMax); isLeafNode = rootNode->isLeafNode(); if (boxBoxOverlap) { @@ -653,39 +623,37 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 //BT_PROFILE("btRayAabb2"); - rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); - + rayBoxOverlap = btRayAabb2(raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); + #else - rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); + rayBoxOverlap = true; //btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); #endif } - + if (isLeafNode && rayBoxOverlap) { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + nodeCallback->processNode(rootNode->getPartId(), rootNode->getTriangleIndex()); } - + //PCK: unsigned instead of bool if ((rayBoxOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + 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 +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; @@ -693,91 +661,85 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb 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) + if (curIndex == drawPatch) { - btVector3 aabbMin,aabbMax; + btVector3 aabbMin, aabbMax; aabbMin = unQuantize(rootNode->m_quantizedAabbMin); aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + btVector3 color(1, 0, 0); + debugDrawerPtr->drawAabb(aabbMin, aabbMax, color); } -#endif//VISUALLY_ANALYZE_BVH +#endif //VISUALLY_ANALYZE_BVH //catch bugs in tree data - btAssert (walkIterations < subTreeSize); + btAssert(walkIterations < subTreeSize); walkIterations++; //PCK: unsigned instead of bool - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode->m_quantizedAabbMin, rootNode->m_quantizedAabbMax); isLeafNode = rootNode->isLeafNode(); - + if (isLeafNode && aabbOverlap) { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); - } - + nodeCallback->processNode(rootNode->getPartId(), rootNode->getTriangleIndex()); + } + //PCK: unsigned instead of bool if ((aabbOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + 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 +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++) + 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); + 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); + 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 +void btQuantizedBvh::reportRayOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const { - reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0)); + reportBoxCastOverlappingNodex(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0)); } - -void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const +void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) const { //always use stackless @@ -801,31 +763,31 @@ void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCa reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); } */ - } - -void btQuantizedBvh::swapLeafNodes(int i,int splitIndex) +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 + 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; + btOptimizedBvhNode tmp = m_leafNodes[i]; + m_leafNodes[i] = m_leafNodes[splitIndex]; + m_leafNodes[splitIndex] = tmp; } } -void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex) { if (m_useQuantization) { m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; - } else + } + else { m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; } @@ -842,11 +804,10 @@ static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; static const unsigned BVH_ALIGNMENT_BLOCKS = 2; #endif - unsigned int btQuantizedBvh::getAlignmentSerializationPadding() { // I changed this to 0 since the extra padding is not needed or used. - return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; + return 0; //BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; } unsigned btQuantizedBvh::calculateSerializeBufferSize() const @@ -860,12 +821,12 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize() const return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); } -bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const +bool btQuantizedBvh::serialize(void* o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const { btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); m_subtreeHeaderCount = m_SubtreeHeaders.size(); -/* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + /* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) { ///check alignedment for buffer? btAssert(0); @@ -873,7 +834,7 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe } */ - btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer; + 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 @@ -883,10 +844,9 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe { 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); + 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)); @@ -903,12 +863,12 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe targetBvh->m_useQuantization = m_useQuantization; - unsigned char *nodeData = (unsigned char *)targetBvh; + unsigned char* nodeData = (unsigned char*)targetBvh; nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + + unsigned sizeToAdd = 0; //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; nodeData += sizeToAdd; - + int nodeCount = m_curNodeIndex; if (m_useQuantization) @@ -934,7 +894,6 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe { 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]; @@ -944,8 +903,6 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe 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; @@ -991,7 +948,7 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0); } - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + sizeToAdd = 0; //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; nodeData += sizeToAdd; // Now serialize the subtree headers @@ -1046,14 +1003,13 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe return true; } -btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +btQuantizedBvh* btQuantizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) { - - if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + if (i_alignedDataBuffer == NULL) // || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) { return NULL; } - btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer; + btQuantizedBvh* bvh = (btQuantizedBvh*)i_alignedDataBuffer; if (i_swapEndian) { @@ -1075,12 +1031,12 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un return NULL; } - unsigned char *nodeData = (unsigned char *)bvh; + unsigned char* nodeData = (unsigned char*)bvh; nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + + 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 @@ -1118,7 +1074,7 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un { 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)); @@ -1127,7 +1083,7 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un nodeData += sizeof(btOptimizedBvhNode) * nodeCount; } - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + sizeToAdd = 0; //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; nodeData += sizeToAdd; // Now serialize the subtree headers @@ -1153,13 +1109,11 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un } // 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), -m_bulletVersion(BT_BULLET_VERSION) +btQuantizedBvh::btQuantizedBvh(btQuantizedBvh& self, bool /* ownsMemory */) : m_bvhAabbMin(self.m_bvhAabbMin), + m_bvhAabbMax(self.m_bvhAabbMax), + m_bvhQuantization(self.m_bvhQuantization), + m_bulletVersion(BT_BULLET_VERSION) { - } void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData) @@ -1169,8 +1123,8 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB m_bvhQuantization.deSerializeFloat(quantizedBvhFloatData.m_bvhQuantization); m_curNodeIndex = quantizedBvhFloatData.m_curNodeIndex; - m_useQuantization = quantizedBvhFloatData.m_useQuantization!=0; - + m_useQuantization = quantizedBvhFloatData.m_useQuantization != 0; + { int numElem = quantizedBvhFloatData.m_numContiguousLeafNodes; m_contiguousNodes.resize(numElem); @@ -1179,7 +1133,7 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB { btOptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_contiguousNodes[i].m_aabbMaxOrg.deSerializeFloat(memPtr->m_aabbMaxOrg); m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg); @@ -1193,11 +1147,11 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB { int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes; m_quantizedContiguousNodes.resize(numElem); - + if (numElem) { btQuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex; m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; @@ -1211,16 +1165,16 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB } m_traversalMode = btTraversalMode(quantizedBvhFloatData.m_traversalMode); - + { int numElem = quantizedBvhFloatData.m_numSubtreeHeaders; m_SubtreeHeaders.resize(numElem); if (numElem) { btBvhSubtreeInfoData* memPtr = quantizedBvhFloatData.m_subTreeInfoPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; @@ -1240,8 +1194,8 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize m_bvhQuantization.deSerializeDouble(quantizedBvhDoubleData.m_bvhQuantization); m_curNodeIndex = quantizedBvhDoubleData.m_curNodeIndex; - m_useQuantization = quantizedBvhDoubleData.m_useQuantization!=0; - + m_useQuantization = quantizedBvhDoubleData.m_useQuantization != 0; + { int numElem = quantizedBvhDoubleData.m_numContiguousLeafNodes; m_contiguousNodes.resize(numElem); @@ -1250,7 +1204,7 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize { btOptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_contiguousNodes[i].m_aabbMaxOrg.deSerializeDouble(memPtr->m_aabbMaxOrg); m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg); @@ -1264,11 +1218,11 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize { int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes; m_quantizedContiguousNodes.resize(numElem); - + if (numElem) { btQuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex; m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; @@ -1282,16 +1236,16 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize } m_traversalMode = btTraversalMode(quantizedBvhDoubleData.m_traversalMode); - + { int numElem = quantizedBvhDoubleData.m_numSubtreeHeaders; m_SubtreeHeaders.resize(numElem); if (numElem) { btBvhSubtreeInfoData* memPtr = quantizedBvhDoubleData.m_subTreeInfoPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { - m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; @@ -1302,52 +1256,51 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize } } } - } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const { btQuantizedBvhData* quantizedData = (btQuantizedBvhData*)dataBuffer; - + m_bvhAabbMax.serialize(quantizedData->m_bvhAabbMax); m_bvhAabbMin.serialize(quantizedData->m_bvhAabbMin); m_bvhQuantization.serialize(quantizedData->m_bvhQuantization); quantizedData->m_curNodeIndex = m_curNodeIndex; quantizedData->m_useQuantization = m_useQuantization; - + quantizedData->m_numContiguousLeafNodes = m_contiguousNodes.size(); - quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*) (m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0); + quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*)(m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0); if (quantizedData->m_contiguousNodesPtr) { int sz = sizeof(btOptimizedBvhNodeData); int numElem = m_contiguousNodes.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btOptimizedBvhNodeData* memPtr = (btOptimizedBvhNodeData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_contiguousNodes[i].m_aabbMaxOrg.serialize(memPtr->m_aabbMaxOrg); m_contiguousNodes[i].m_aabbMinOrg.serialize(memPtr->m_aabbMinOrg); memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex; memPtr->m_subPart = m_contiguousNodes[i].m_subPart; memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex; + // Fill padding with zeros to appease msan. + memset(memPtr->m_pad, 0, sizeof(memPtr->m_pad)); } - serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]); + serializer->finalizeChunk(chunk, "btOptimizedBvhNodeData", BT_ARRAY_CODE, (void*)&m_contiguousNodes[0]); } quantizedData->m_numQuantizedContiguousNodes = m_quantizedContiguousNodes.size(); -// printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes); - quantizedData->m_quantizedContiguousNodesPtr =(btQuantizedBvhNodeData*) (m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0); + // printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes); + quantizedData->m_quantizedContiguousNodesPtr = (btQuantizedBvhNodeData*)(m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0); if (quantizedData->m_quantizedContiguousNodesPtr) { int sz = sizeof(btQuantizedBvhNodeData); int numElem = m_quantizedContiguousNodes.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btQuantizedBvhNodeData* memPtr = (btQuantizedBvhNodeData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex; memPtr->m_quantizedAabbMax[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[0]; @@ -1357,20 +1310,20 @@ const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer memPtr->m_quantizedAabbMin[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[1]; memPtr->m_quantizedAabbMin[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[2]; } - serializer->finalizeChunk(chunk,"btQuantizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_quantizedContiguousNodes[0]); + serializer->finalizeChunk(chunk, "btQuantizedBvhNodeData", BT_ARRAY_CODE, (void*)&m_quantizedContiguousNodes[0]); } quantizedData->m_traversalMode = int(m_traversalMode); quantizedData->m_numSubtreeHeaders = m_SubtreeHeaders.size(); - quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*) (m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0); + quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*)(m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0); if (quantizedData->m_subTreeInfoPtr) { int sz = sizeof(btBvhSubtreeInfoData); int numElem = m_SubtreeHeaders.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btBvhSubtreeInfoData* memPtr = (btBvhSubtreeInfoData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_quantizedAabbMax[0] = m_SubtreeHeaders[i].m_quantizedAabbMax[0]; memPtr->m_quantizedAabbMax[1] = m_SubtreeHeaders[i].m_quantizedAabbMax[1]; @@ -1382,12 +1335,7 @@ const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer memPtr->m_rootNodeIndex = m_SubtreeHeaders[i].m_rootNodeIndex; memPtr->m_subtreeSize = m_SubtreeHeaders[i].m_subtreeSize; } - serializer->finalizeChunk(chunk,"btBvhSubtreeInfoData",BT_ARRAY_CODE,(void*)&m_SubtreeHeaders[0]); + serializer->finalizeChunk(chunk, "btBvhSubtreeInfoData", BT_ARRAY_CODE, (void*)&m_SubtreeHeaders[0]); } return btQuantizedBvhDataName; } - - - - - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index 78382da79f0..1c47b9ccf29 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -22,11 +22,11 @@ class btSerializer; #ifdef DEBUG_CHECK_DEQUANTIZATION #ifdef __SPU__ #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #include <stdio.h> #include <stdlib.h> -#endif //DEBUG_CHECK_DEQUANTIZATION +#endif //DEBUG_CHECK_DEQUANTIZATION #include "LinearMath/btVector3.h" #include "LinearMath/btAlignedAllocator.h" @@ -41,13 +41,10 @@ class btSerializer; #define btQuantizedBvhDataName "btQuantizedBvhFloatData" #endif - - //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 +#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 @@ -55,15 +52,16 @@ class btSerializer; ///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 +ATTRIBUTE_ALIGNED16(struct) +btQuantizedBvhNode { BT_DECLARE_ALIGNED_ALLOCATOR(); //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; //4 bytes - int m_escapeIndexOrTriangleIndex; + int m_escapeIndexOrTriangleIndex; bool isLeafNode() const { @@ -75,68 +73,67 @@ ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode btAssert(!isLeafNode()); return -m_escapeIndexOrTriangleIndex; } - int getTriangleIndex() const + int getTriangleIndex() const { btAssert(isLeafNode()); - unsigned int x=0; - unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + unsigned int x = 0; + unsigned int y = (~(x & 0)) << (31 - MAX_NUM_PARTS_IN_BITS); // Get only the lower bits where the triangle index is stored - return (m_escapeIndexOrTriangleIndex&~(y)); + return (m_escapeIndexOrTriangleIndex & ~(y)); } - int getPartId() const + 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)); + 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 +ATTRIBUTE_ALIGNED16(struct) +btOptimizedBvhNode { BT_DECLARE_ALIGNED_ALLOCATOR(); //32 bytes - btVector3 m_aabbMinOrg; - btVector3 m_aabbMaxOrg; + btVector3 m_aabbMinOrg; + btVector3 m_aabbMaxOrg; //4 - int m_escapeIndex; + int m_escapeIndex; //8 //for child nodes - int m_subPart; - int m_triangleIndex; + int m_subPart; + int m_triangleIndex; -//pad the size to 64 bytes - char m_padding[20]; + //pad the size to 64 bytes + char m_padding[20]; }; - ///btBvhSubtreeInfo provides info to gather a subtree of limited size -ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo +ATTRIBUTE_ALIGNED16(class) +btBvhSubtreeInfo { public: BT_DECLARE_ALIGNED_ALLOCATOR(); //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; //4 bytes, points to the root of the subtree - int m_rootNodeIndex; + int m_rootNodeIndex; //4 bytes - int m_subtreeSize; - int m_padding[3]; + int m_subtreeSize; + int m_padding[3]; btBvhSubtreeInfo() { //memset(&m_padding[0], 0, sizeof(m_padding)); } - - void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) + void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) { m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; @@ -145,14 +142,12 @@ public: m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; } -} -; - +}; class btNodeOverlapCallback { public: - virtual ~btNodeOverlapCallback() {}; + virtual ~btNodeOverlapCallback(){}; virtual void processNode(int subPart, int triangleIndex) = 0; }; @@ -160,18 +155,16 @@ public: #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedObjectArray.h" - - ///for code readability: -typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray; -typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray; -typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray; - +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 used by the btBvhTriangleMeshShape as midphase. ///It is recommended to use quantization for better performance and lower memory requirements. -ATTRIBUTE_ALIGNED16(class) btQuantizedBvh +ATTRIBUTE_ALIGNED16(class) +btQuantizedBvh { public: enum btTraversalMode @@ -182,54 +175,47 @@ public: }; protected: + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; + int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. - btVector3 m_bvhAabbMin; - btVector3 m_bvhAabbMax; - btVector3 m_bvhQuantization; - - int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. - - int m_curNodeIndex; + int m_curNodeIndex; //quantization data - bool m_useQuantization; + bool m_useQuantization; + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + QuantizedNodeArray m_quantizedLeafNodes; + QuantizedNodeArray m_quantizedContiguousNodes; - - NodeArray m_leafNodes; - NodeArray m_contiguousNodes; - QuantizedNodeArray m_quantizedLeafNodes; - QuantizedNodeArray m_quantizedContiguousNodes; - - btTraversalMode m_traversalMode; - BvhSubtreeInfoArray m_SubtreeHeaders; + btTraversalMode m_traversalMode; + BvhSubtreeInfoArray m_SubtreeHeaders; //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray mutable 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) + void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) { if (m_useQuantization) { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); - } else + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0], aabbMin, 0); + } + else { m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; - } } - void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) + void setInternalNodeAabbMax(int nodeIndex, const btVector3& aabbMax) { if (m_useQuantization) { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); - } else + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0], aabbMax, 1); + } + else { m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; } @@ -243,115 +229,102 @@ protected: } //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) + 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) + 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++) + 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 + } + else { //non-quantized m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); - m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); + m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); } } - void swapLeafNodes(int firstIndex,int secondIndex); + void swapLeafNodes(int firstIndex, int secondIndex); - void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); + void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex); protected: + void buildTree(int startIndex, int endIndex); - + int calcSplittingAxis(int startIndex, int endIndex); - void buildTree (int startIndex,int endIndex); + int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis); - int calcSplittingAxis(int startIndex,int endIndex); + void walkStacklessTree(btNodeOverlapCallback * nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const; - 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; - void walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) 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; + void walkStacklessTreeAgainstRay(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, 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; + 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; + 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; - + void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA, const btQuantizedBvhNode* treeNodeB, btNodeOverlapCallback* nodeCallback) const; - - - void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); 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; } + 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(); + 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; + 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 + SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point, int isMax) const { - btAssert(m_useQuantization); btAssert(point.getX() <= m_bvhAabbMax.getX()); @@ -368,122 +341,114 @@ public: ///@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() + 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)); + 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()); + 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()); + 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()); + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ()); } - } else + } + else { if (newPoint.getX() > point.getX()) { - printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), 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()); + 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()); + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ()); } } -#endif //DEBUG_CHECK_DEQUANTIZATION - +#endif //DEBUG_CHECK_DEQUANTIZATION } - - SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const + 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); - + quantize(out, clampedPoint, isMax); } - - SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const + + SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const { - btVector3 vecOut; - vecOut.setValue( + 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; + 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) + void setTraversalMode(btTraversalMode traversalMode) { m_traversalMode = traversalMode; } - - SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() - { - return m_quantizedContiguousNodes; + SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; } - - SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() + SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() { return m_SubtreeHeaders; } -//////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////// /////Calculate space needed to store BVH for serialization unsigned calculateSerializeBufferSize() const; /// Data buffer MUST be 16 byte aligned - virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const; + virtual bool serialize(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const; ///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 btQuantizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); static unsigned int getAlignmentSerializationPadding(); -////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////// - - virtual int calculateSerializeBufferSizeNew() const; + virtual int calculateSerializeBufferSizeNew() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData); + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData); + virtual void deSerializeFloat(struct btQuantizedBvhFloatData & quantizedBvhFloatData); + virtual void deSerializeDouble(struct btQuantizedBvhDoubleData & quantizedBvhDoubleData); -//////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////// SIMD_FORCE_INLINE bool isQuantized() { @@ -494,38 +459,37 @@ 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); - -} -; - + btQuantizedBvh(btQuantizedBvh & other, bool ownsMemory); +}; -struct btBvhSubtreeInfoData +// clang-format off +// parser needs * with the name +struct btBvhSubtreeInfoData { - int m_rootNodeIndex; - int m_subtreeSize; + int m_rootNodeIndex; + int m_subtreeSize; unsigned short m_quantizedAabbMin[3]; unsigned short m_quantizedAabbMax[3]; }; struct btOptimizedBvhNodeFloatData { - btVector3FloatData m_aabbMinOrg; - btVector3FloatData m_aabbMaxOrg; - int m_escapeIndex; - int m_subPart; - int m_triangleIndex; + btVector3FloatData m_aabbMinOrg; + btVector3FloatData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; char m_pad[4]; }; struct btOptimizedBvhNodeDoubleData { - btVector3DoubleData m_aabbMinOrg; - btVector3DoubleData m_aabbMaxOrg; - int m_escapeIndex; - int m_subPart; - int m_triangleIndex; - char m_pad[4]; + btVector3DoubleData m_aabbMinOrg; + btVector3DoubleData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; }; @@ -569,13 +533,11 @@ struct btQuantizedBvhDoubleData int m_numSubtreeHeaders; btBvhSubtreeInfoData *m_subTreeInfoPtr; }; +// clang-format on - -SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const +SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const { return sizeof(btQuantizedBvhData); } - - -#endif //BT_QUANTIZED_BVH_H +#endif //BT_QUANTIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 752fcd0fef2..b7fe0a1f341 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -24,52 +24,45 @@ subject to the following restrictions: #include <new> -extern int gOverlappingPairs; - -void btSimpleBroadphase::validate() +void btSimpleBroadphase::validate() { - for (int i=0;i<m_numHandles;i++) + for (int i = 0; i < m_numHandles; i++) { - for (int j=i+1;j<m_numHandles;j++) + for (int j = i + 1; j < m_numHandles; j++) { btAssert(&m_pHandles[i] != &m_pHandles[j]); } } - } btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache) - :m_pairCache(overlappingPairCache), - m_ownsPairCache(false), - m_invalidPair(0) + : m_pairCache(overlappingPairCache), + m_ownsPairCache(false), + m_invalidPair(0) { - if (!overlappingPairCache) { - void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16); - m_pairCache = new (mem)btHashedOverlappingPairCache(); + 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_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy) * maxProxies, 16); + m_pHandles = new (m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies]; m_maxHandles = maxProxies; m_numHandles = 0; m_firstFreeHandle = 0; m_LastHandleIndex = -1; - { 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[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() @@ -83,26 +76,25 @@ btSimpleBroadphase::~btSimpleBroadphase() } } - -btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy) +btBroadphaseProxy* btSimpleBroadphase::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/) { if (m_numHandles >= m_maxHandles) { btAssert(0); - return 0; //should never happen, but don't let the game crash ;-) + return 0; //should never happen, but don't let the game crash ;-) } - btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]); + btAssert(aabbMin[0] <= aabbMax[0] && aabbMin[1] <= aabbMax[1] && aabbMin[2] <= aabbMax[2]); int newHandleIndex = allocHandle(); - btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy); + btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex]) btSimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask); return proxy; } -class RemovingOverlapCallback : public btOverlapCallback +class RemovingOverlapCallback : public btOverlapCallback { protected: - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { (void)pair; btAssert(0); @@ -112,12 +104,13 @@ protected: class RemovePairContainingProxy { + btBroadphaseProxy* m_targetProxy; - btBroadphaseProxy* m_targetProxy; - public: +public: virtual ~RemovePairContainingProxy() { } + protected: virtual bool processOverlap(btBroadphasePair& pair) { @@ -128,38 +121,36 @@ protected: }; }; -void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher) +void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher) { - - btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg); - freeHandle(proxy0); + m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher); - m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher); + btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg); + freeHandle(proxy0); - //validate(); - + //validate(); } -void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const { const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); aabbMin = sbp->m_aabbMin; aabbMax = sbp->m_aabbMax; } -void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/) +void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/) { btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); sbp->m_aabbMin = aabbMin; sbp->m_aabbMax = aabbMax; } -void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) +void btSimpleBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax) { - for (int i=0; i <= m_LastHandleIndex; i++) + for (int i = 0; i <= m_LastHandleIndex; i++) { btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; - if(!proxy->m_clientObject) + if (!proxy->m_clientObject) { continue; } @@ -167,69 +158,59 @@ void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo } } - -void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) { - for (int i=0; i <= m_LastHandleIndex; i++) + for (int i = 0; i <= m_LastHandleIndex; i++) { btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; - if(!proxy->m_clientObject) + if (!proxy->m_clientObject) { continue; } - if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax)) + if (TestAabbAgainstAabb2(aabbMin, aabbMax, proxy->m_aabbMin, proxy->m_aabbMax)) { callback.process(proxy); } } } - - - - - - -bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1) +bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1) { - return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && + return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] && proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2]; - } - - //then remove non-overlapping ones class CheckOverlapCallback : public btOverlapCallback { public: virtual bool processOverlap(btBroadphasePair& pair) { - return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1))); + return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0), static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1))); } }; -void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { //first check for new overlapping pairs - int i,j; + int i, j; if (m_numHandles >= 0) { int new_largest_index = -1; - for (i=0; i <= m_LastHandleIndex; i++) + for (i = 0; i <= m_LastHandleIndex; i++) { btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i]; - if(!proxy0->m_clientObject) + if (!proxy0->m_clientObject) { continue; } new_largest_index = i; - for (j=i+1; j <= m_LastHandleIndex; j++) + for (j = i + 1; j <= m_LastHandleIndex; j++) { btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j]; btAssert(proxy0 != proxy1); - if(!proxy1->m_clientObject) + if (!proxy1->m_clientObject) { continue; } @@ -237,19 +218,20 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); - if (aabbOverlap(p0,p1)) + if (aabbOverlap(p0, p1)) { - if ( !m_pairCache->findPair(proxy0,proxy1)) + if (!m_pairCache->findPair(proxy0, proxy1)) { - m_pairCache->addOverlappingPair(proxy0,proxy1); + m_pairCache->addOverlappingPair(proxy0, proxy1); } - } else + } + else { if (!m_pairCache->hasDeferredRemoval()) { - if ( m_pairCache->findPair(proxy0,proxy1)) + if (m_pairCache->findPair(proxy0, proxy1)) { - m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher); + m_pairCache->removeOverlappingPair(proxy0, proxy1, dispatcher); } } } @@ -260,8 +242,7 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) if (m_ownsPairCache && m_pairCache->hasDeferredRemoval()) { - - btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); + btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); //perform a sort, to find duplicates and to sort 'invalid' pairs to the end overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); @@ -269,16 +250,13 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) 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++) + for (i = 0; i < overlappingPairArray.size(); i++) { - btBroadphasePair& pair = overlappingPairArray[i]; bool isDuplicate = (pair == previousPair); @@ -289,16 +267,18 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) if (!isDuplicate) { - bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); + bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1); if (hasOverlap) { - needsRemoval = false;//callback->processOverlap(pair); - } else + needsRemoval = false; //callback->processOverlap(pair); + } + else { needsRemoval = true; } - } else + } + else { //remove duplicate needsRemoval = true; @@ -308,16 +288,14 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) if (needsRemoval) { - m_pairCache->cleanOverlappingPair(pair,dispatcher); + 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: @@ -329,21 +307,19 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); m_invalidPair = 0; -#endif//CLEAN_INVALID_PAIRS - +#endif //CLEAN_INVALID_PAIRS } } } - -bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); - return aabbOverlap(p0,p1); + return aabbOverlap(p0, p1); } -void btSimpleBroadphase::resetPool(btDispatcher* dispatcher) +void btSimpleBroadphase::resetPool(btDispatcher* dispatcher) { //not yet } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 7cb3c40a043..3e02fdc0036 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -16,57 +16,47 @@ subject to the following restrictions: #ifndef BT_SIMPLE_BROADPHASE_H #define BT_SIMPLE_BROADPHASE_H - #include "btOverlappingPairCache.h" - struct btSimpleBroadphaseProxy : public btBroadphaseProxy { - int m_nextFree; - -// int m_handleId; + int m_nextFree; - - btSimpleBroadphaseProxy() {}; + // int m_handleId; - btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy) - :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy) + btSimpleBroadphaseProxy(){}; + + btSimpleBroadphaseProxy(const btVector3& minpt, const btVector3& maxpt, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask) + : btBroadphaseProxy(minpt, maxpt, userPtr, collisionFilterGroup, collisionFilterMask) { (void)shapeType; } - - - SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;} - SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;} - - - + SIMD_FORCE_INLINE void SetNextFree(int next) { m_nextFree = next; } + SIMD_FORCE_INLINE int GetNextFree() const { return m_nextFree; } }; ///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: + int m_numHandles; // number of active handles + int m_maxHandles; // max number of handles + int m_LastHandleIndex; - int m_numHandles; // number of active handles - int m_maxHandles; // max number of handles - int m_LastHandleIndex; - - btSimpleBroadphaseProxy* m_pHandles; // handles pool + btSimpleBroadphaseProxy* m_pHandles; // handles pool void* m_pHandlesRawPtr; - int m_firstFreeHandle; // free handles list - + 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++; - if(freeHandle > m_LastHandleIndex) + if (freeHandle > m_LastHandleIndex) { m_LastHandleIndex = freeHandle; } @@ -75,9 +65,9 @@ protected: void freeHandle(btSimpleBroadphaseProxy* proxy) { - int handle = int(proxy-m_pHandles); + int handle = int(proxy - m_pHandles); btAssert(handle >= 0 && handle < m_maxHandles); - if(handle == m_LastHandleIndex) + if (handle == m_LastHandleIndex) { m_LastHandleIndex--; } @@ -89,20 +79,18 @@ protected: m_numHandles--; } - btOverlappingPairCache* m_pairCache; - bool m_ownsPairCache; + btOverlappingPairCache* m_pairCache; + bool m_ownsPairCache; - int m_invalidPair; + int m_invalidPair; - - - inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) + inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) { btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy); return proxy0; } - inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const + inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const { const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy); return proxy0; @@ -111,61 +99,50 @@ protected: ///reset broadphase internal structures, to ensure determinism/reproducability virtual void resetPool(btDispatcher* dispatcher); - - void validate(); + void validate(); protected: - - - - public: - btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0); + btSimpleBroadphase(int maxProxies = 16384, btOverlappingPairCache* overlappingPairCache = 0); virtual ~btSimpleBroadphase(); + static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1); - static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1); - + virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher); - 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 calculateOverlappingPairs(btDispatcher* dispatcher); + virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher); + virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - - btOverlappingPairCache* getOverlappingPairCache() + btOverlappingPairCache* getOverlappingPairCache() { return m_pairCache; } - const btOverlappingPairCache* getOverlappingPairCache() const + const btOverlappingPairCache* getOverlappingPairCache() const { return m_pairCache; } - bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - + 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 + virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const { - aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); - aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); + aabbMin.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT); + aabbMax.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); } - virtual void printStats() + virtual void printStats() { -// printf("btSimpleBroadphase.h\n"); -// printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); + // printf("btSimpleBroadphase.h\n"); + // printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); } }; - - -#endif //BT_SIMPLE_BROADPHASE_H - +#endif //BT_SIMPLE_BROADPHASE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 63401780970..7647f673600 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -18,145 +18,162 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" - -SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) -:m_sphere(sphere), -m_triangle(triangle), -m_contactBreakingThreshold(contactBreakingThreshold) +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold) + : m_sphere(sphere), + m_triangle(triangle), + m_contactBreakingThreshold(contactBreakingThreshold) { - } -void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults) { - (void)debugDraw; const btTransform& transformA = input.m_transformA; const btTransform& transformB = input.m_transformB; - btVector3 point,normal; + btVector3 point, normal; btScalar timeOfImpact = btScalar(1.); btScalar depth = btScalar(0.); -// output.m_distance = btScalar(BT_LARGE_FLOAT); + // output.m_distance = btScalar(BT_LARGE_FLOAT); //move sphere into triangle space - btTransform sphereInTr = transformB.inverseTimes(transformA); + btTransform sphereInTr = transformB.inverseTimes(transformA); - if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) + if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold)) { if (swapResults) { - btVector3 normalOnB = transformB.getBasis()*normal; + btVector3 normalOnB = transformB.getBasis() * normal; btVector3 normalOnA = -normalOnB; - btVector3 pointOnA = transformB*point+normalOnB*depth; - output.addContactPoint(normalOnA,pointOnA,depth); - } else + btVector3 pointOnA = transformB * point + normalOnB * depth; + output.addContactPoint(normalOnA, pointOnA, depth); + } + else { - output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth); } } - } - - // 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); -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; btScalar t = v.dot(diff); - - if (t > 0) { + + if (t > 0) + { btScalar dotVV = v.dot(v); - if (t < dotVV) { + if (t < dotVV) + { t /= dotVV; - diff -= t*v; - } else { + diff -= t * v; + } + else + { t = 1; diff -= v; } - } else + } + else t = 0; - nearest = from + t*v; - return diff.dot(diff); + nearest = from + t * v; + return diff.dot(diff); } -bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { +bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal) +{ btVector3 lp(p); btVector3 lnormal(normal); - + return pointInTriangle(vertices, lnormal, &lp); } -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) +bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold) { - const btVector3* vertices = &m_triangle->getVertexPtr(0); - + btScalar radius = m_sphere->getRadius(); btScalar radiusWithThreshold = radius + contactBreakingThreshold; - btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); - normal.normalize(); - btVector3 p1ToCentre = sphereCenter - vertices[0]; - btScalar distanceFromPlane = p1ToCentre.dot(normal); - - if (distanceFromPlane < btScalar(0.)) - { - //triangle facing the other way - distanceFromPlane *= btScalar(-1.); - normal *= btScalar(-1.); - } + btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]); - bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - - // Check for contact / intersection + btScalar l2 = normal.length2(); bool hasContact = false; btVector3 contactPoint; - if (isInsideContactPlane) { - if (facecontains(sphereCenter,vertices,normal)) { - // Inside the contact wedge - touches a point on the shell plane - hasContact = true; - contactPoint = sphereCenter - normal*distanceFromPlane; - } else { - // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; - btVector3 nearestOnEdge; - for (int i = 0; i < m_triangle->getNumEdges(); i++) { - - btVector3 pa; - btVector3 pb; - - m_triangle->getEdge(i,pa,pb); - - btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge); - if (distanceSqr < contactCapsuleRadiusSqr) { - // Yep, we're inside a capsule - hasContact = true; - contactPoint = nearestOnEdge; + + if (l2 >= SIMD_EPSILON * SIMD_EPSILON) + { + normal /= btSqrt(l2); + + btVector3 p1ToCentre = sphereCenter - vertices[0]; + btScalar distanceFromPlane = p1ToCentre.dot(normal); + + if (distanceFromPlane < btScalar(0.)) + { + //triangle facing the other way + distanceFromPlane *= btScalar(-1.); + normal *= btScalar(-1.); + } + + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; + + // Check for contact / intersection + + if (isInsideContactPlane) + { + if (facecontains(sphereCenter, vertices, normal)) + { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = sphereCenter - normal * distanceFromPlane; + } + else + { + // Could be inside one of the contact capsules + btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold; + btScalar minDistSqr = contactCapsuleRadiusSqr; + btVector3 nearestOnEdge; + for (int i = 0; i < m_triangle->getNumEdges(); i++) + { + btVector3 pa; + btVector3 pb; + + m_triangle->getEdge(i, pa, pb); + + btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge); + if (distanceSqr < minDistSqr) + { + // Yep, we're inside a capsule, and record the capsule with smallest distance + minDistSqr = distanceSqr; + hasContact = true; + contactPoint = nearestOnEdge; + } } - } } } - if (hasContact) { + if (hasContact) + { btVector3 contactToCentre = sphereCenter - contactPoint; btScalar distanceSqr = contactToCentre.length2(); - if (distanceSqr < radiusWithThreshold*radiusWithThreshold) + if (distanceSqr < radiusWithThreshold * radiusWithThreshold) { - if (distanceSqr>SIMD_EPSILON) + if (distanceSqr > SIMD_EPSILON) { btScalar distance = btSqrt(distanceSqr); resultNormal = contactToCentre; resultNormal.normalize(); point = contactPoint; - depth = -(radius-distance); - } else + depth = -(radius - distance); + } + else { resultNormal = normal; point = contactPoint; @@ -165,36 +182,34 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po return true; } } - + return false; } - -bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) +bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p) { const btVector3* p1 = &vertices[0]; const btVector3* p2 = &vertices[1]; const btVector3* p3 = &vertices[2]; - btVector3 edge1( *p2 - *p1 ); - btVector3 edge2( *p3 - *p2 ); - btVector3 edge3( *p1 - *p3 ); + btVector3 edge1(*p2 - *p1); + btVector3 edge2(*p3 - *p2); + btVector3 edge3(*p1 - *p3); - btVector3 p1_to_p( *p - *p1 ); - btVector3 p2_to_p( *p - *p2 ); - btVector3 p3_to_p( *p - *p3 ); + btVector3 p1_to_p(*p - *p1); + btVector3 p2_to_p(*p - *p2); + btVector3 p3_to_p(*p - *p3); + + btVector3 edge1_normal(edge1.cross(normal)); + btVector3 edge2_normal(edge2.cross(normal)); + btVector3 edge3_normal(edge3.cross(normal)); - btVector3 edge1_normal( edge1.cross(normal)); - btVector3 edge2_normal( edge2.cross(normal)); - btVector3 edge3_normal( edge3.cross(normal)); - btScalar r1, r2, r3; - r1 = edge1_normal.dot( p1_to_p ); - r2 = edge2_normal.dot( p2_to_p ); - r3 = edge3_normal.dot( p3_to_p ); - if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || - ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) + r1 = edge1_normal.dot(p1_to_p); + r2 = edge2_normal.dot(p2_to_p); + r3 = edge3_normal.dot(p3_to_p); + if ((r1 > 0 && r2 > 0 && r3 > 0) || + (r1 <= 0 && r2 <= 0 && r3 <= 0)) return true; return false; - } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 22953af43fd..d47e47530d4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -18,34 +18,26 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - - class btSphereShape; class btTriangleShape; - - /// sphere-triangle to match the btDiscreteCollisionDetectorInterface struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface { - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); - SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold); + SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold); - virtual ~SphereTriangleDetector() {}; + virtual ~SphereTriangleDetector(){}; - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + bool collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold); private: - - - bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); - bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); + bool pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p); + bool facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal); btSphereShape* m_sphere; btTriangleShape* m_triangle; - btScalar m_contactBreakingThreshold; - + btScalar m_contactBreakingThreshold; }; -#endif //BT_SPHERE_TRIANGLE_DETECTOR_H - +#endif //BT_SPHERE_TRIANGLE_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp index 57f14649353..ac5de45d271 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -17,31 +17,31 @@ subject to the following restrictions: #include "btCollisionDispatcher.h" #include "btCollisionObject.h" -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) -:btCollisionAlgorithm(ci) +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) //, //m_colObj0(0), //m_colObj1(0) { } -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ) -:btCollisionAlgorithm(ci) +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper*, const btCollisionObjectWrapper*) + : btCollisionAlgorithm(ci) //, //m_colObj0(0), //m_colObj1(0) { -// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) -// { -// m_colObj0 = colObj0; -// m_colObj1 = colObj1; -// -// m_colObj0->activate(); -// m_colObj1->activate(); -// } + // if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) + // { + // m_colObj0 = colObj0; + // m_colObj1 = colObj1; + // + // m_colObj0->activate(); + // m_colObj1->activate(); + // } } btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() { -// m_colObj0->activate(); -// m_colObj1->activate(); + // m_colObj0->activate(); + // m_colObj1->activate(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h index 489812b9663..862060571b5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -21,16 +21,15 @@ subject to the following restrictions: ///This class is not enabled yet (work-in-progress) to more aggressively activate objects. class btActivatingCollisionAlgorithm : public btCollisionAlgorithm { -// btCollisionObject* m_colObj0; -// btCollisionObject* m_colObj1; + // btCollisionObject* m_colObj0; + // btCollisionObject* m_colObj1; -public: - - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); +protected: + btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); +public: virtual ~btActivatingCollisionAlgorithm(); - }; -#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H +#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index c1da9f36cfb..6873a95d90a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -26,61 +26,55 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 -btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap) -: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap) + : btActivatingCollisionAlgorithm(ci, obj0Wrap, obj1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { - if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject()); m_ownManifold = true; } } btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() { - if (m_ownManifold) { if (m_manifoldPtr) m_dispatcher->releaseManifold(m_manifoldPtr); } - } - -void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); +void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); //#include <stdio.h> -void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btBox2dBox2dCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape(); const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape(); resultOut->setPersistentManifold(m_manifoldPtr); - b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform()); + b2CollidePolygons(resultOut, box0, body0Wrap->getWorldTransform(), box1, body1Wrap->getWorldTransform()); // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added if (m_ownManifold) { resultOut->refreshContactPoints(); } - } -btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { //not yet return 1.f; } - struct ClipVertex { btVector3 v; @@ -89,16 +83,16 @@ struct ClipVertex //b2ContactID id; }; -#define b2Dot(a,b) (a).dot(b) -#define b2Mul(a,b) (a)*(b) -#define b2MulT(a,b) (a).transpose()*(b) -#define b2Cross(a,b) (a).cross(b) -#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f) +#define b2Dot(a, b) (a).dot(b) +#define b2Mul(a, b) (a) * (b) +#define b2MulT(a, b) (a).transpose() * (b) +#define b2Cross(a, b) (a).cross(b) +#define btCrossS(a, s) btVector3(s* a.getY(), -s* a.getX(), 0.f) -int b2_maxManifoldPoints =2; +int b2_maxManifoldPoints = 2; static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], - const btVector3& normal, btScalar offset) + const btVector3& normal, btScalar offset) { // Start with no output points int numOut = 0; @@ -133,7 +127,7 @@ static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], // Find the separation between poly1 and poly2 for a give edge normal on poly1. static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1, - const btBox2dShape* poly2, const btTransform& xf2) + const btBox2dShape* poly2, const btTransform& xf2) { const btVector3* vertices1 = poly1->getVertices(); const btVector3* normals1 = poly1->getNormals(); @@ -151,8 +145,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1 int index = 0; btScalar minDot = BT_LARGE_FLOAT; - if( count2 > 0 ) - index = (int) normal1.minDot( vertices2, count2, minDot); + if (count2 > 0) + index = (int)normal1.minDot(vertices2, count2, minDot); btVector3 v1 = b2Mul(xf1, vertices1[edge1]); btVector3 v2 = b2Mul(xf2, vertices2[index]); @@ -162,8 +156,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1 // Find the max separation between poly1 and poly2 using edge normals from poly1. static btScalar FindMaxSeparation(int* edgeIndex, - const btBox2dShape* poly1, const btTransform& xf1, - const btBox2dShape* poly2, const btTransform& xf2) + const btBox2dShape* poly1, const btTransform& xf1, + const btBox2dShape* poly2, const btTransform& xf2) { int count1 = poly1->getVertexCount(); const btVector3* normals1 = poly1->getNormals(); @@ -175,8 +169,8 @@ static btScalar FindMaxSeparation(int* edgeIndex, // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; btScalar maxDot; - if( count1 > 0 ) - edge = (int) dLocal1.maxDot( normals1, count1, maxDot); + if (count1 > 0) + edge = (int)dLocal1.maxDot(normals1, count1, maxDot); // Get the separation for the edge normal. btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); @@ -224,7 +218,7 @@ static btScalar FindMaxSeparation(int* edgeIndex, } // Perform a local search for the best edge normal. - for ( ; ; ) + for (;;) { if (increment == -1) edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; @@ -285,14 +279,14 @@ static void FindIncidentEdge(ClipVertex c[2], int i2 = i1 + 1 < count2 ? i1 + 1 : 0; c[0].v = b2Mul(xf2, vertices2[i1]); -// c[0].id.features.referenceEdge = (unsigned char)edge1; -// c[0].id.features.incidentEdge = (unsigned char)i1; -// c[0].id.features.incidentVertex = 0; + // c[0].id.features.referenceEdge = (unsigned char)edge1; + // c[0].id.features.incidentEdge = (unsigned char)i1; + // c[0].id.features.incidentVertex = 0; c[1].v = b2Mul(xf2, vertices2[i2]); -// c[1].id.features.referenceEdge = (unsigned char)edge1; -// c[1].id.features.incidentEdge = (unsigned char)i2; -// c[1].id.features.incidentVertex = 1; + // c[1].id.features.referenceEdge = (unsigned char)edge1; + // c[1].id.features.incidentEdge = (unsigned char)i2; + // c[1].id.features.incidentVertex = 1; } // Find edge normal of max separation on A - return if separating axis is found @@ -303,10 +297,9 @@ static void FindIncidentEdge(ClipVertex c[2], // The normal points from 1 to 2 void b2CollidePolygons(btManifoldResult* manifold, - const btBox2dShape* polyA, const btTransform& xfA, - const btBox2dShape* polyB, const btTransform& xfB) + const btBox2dShape* polyA, const btTransform& xfA, + const btBox2dShape* polyB, const btTransform& xfB) { - int edgeA = 0; btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); if (separationA > 0.0f) @@ -317,10 +310,10 @@ void b2CollidePolygons(btManifoldResult* manifold, if (separationB > 0.0f) return; - const btBox2dShape* poly1; // reference poly - const btBox2dShape* poly2; // incident poly + const btBox2dShape* poly1; // reference poly + const btBox2dShape* poly2; // incident poly btTransform xf1, xf2; - int edge1; // reference edge + int edge1; // reference edge unsigned char flip; const btScalar k_relativeTol = 0.98f; const btScalar k_absoluteTol = 0.001f; @@ -352,14 +345,13 @@ void b2CollidePolygons(btManifoldResult* manifold, const btVector3* vertices1 = poly1->getVertices(); btVector3 v11 = vertices1[edge1]; - btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; + btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; //btVector3 dv = v12 - v11; btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11); sideNormal.normalize(); btVector3 frontNormal = btCrossS(sideNormal, 1.0f); - - + v11 = b2Mul(xf1, v11); v12 = b2Mul(xf1, v12); @@ -369,13 +361,12 @@ void b2CollidePolygons(btManifoldResult* manifold, // Clip incident edge against extruded edge1 side edges. ClipVertex clipPoints1[2]; - clipPoints1[0].v.setValue(0,0,0); - clipPoints1[1].v.setValue(0,0,0); + clipPoints1[0].v.setValue(0, 0, 0); + clipPoints1[1].v.setValue(0, 0, 0); ClipVertex clipPoints2[2]; - clipPoints2[0].v.setValue(0,0,0); - clipPoints2[1].v.setValue(0,0,0); - + clipPoints2[0].v.setValue(0, 0, 0); + clipPoints2[1].v.setValue(0, 0, 0); int np; @@ -386,7 +377,7 @@ void b2CollidePolygons(btManifoldResult* manifold, return; // Clip to negative box side 1 - np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); + np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); if (np < 2) { @@ -403,19 +394,18 @@ void b2CollidePolygons(btManifoldResult* manifold, if (separation <= 0.0f) { - //b2ManifoldPoint* cp = manifold->points + pointCount; //btScalar separation = separation; //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v); //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v); - manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation); + manifold->addContactPoint(-manifoldNormal, clipPoints2[i].v, separation); -// cp->id = clipPoints2[i].id; -// cp->id.features.flip = flip; + // cp->id = clipPoints2[i].id; + // cp->id.features.flip = flip; ++pointCount; } } -// manifold->pointCount = pointCount;} + // manifold->pointCount = pointCount;} } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h index 6ea6e89bda6..3b66d1fd0b9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -26,22 +26,22 @@ class btPersistentManifold; ///box-box collision detection class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btBox2dBox2dCollisionAlgorithm(); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -49,18 +49,15 @@ public: } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + return new (ptr) btBox2dBox2dCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H - +#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index ac68968f590..7a391e059a7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -21,14 +21,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" #define USE_PERSISTENT_CONTACTS 1 -btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { - if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -42,30 +42,27 @@ btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() } } -void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btBoxBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btBoxShape* box0 = (btBoxShape*)body0Wrap->getCollisionShape(); const btBoxShape* box1 = (btBoxShape*)body1Wrap->getCollisionShape(); - - /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); -#ifndef USE_PERSISTENT_CONTACTS +#ifndef USE_PERSISTENT_CONTACTS m_manifoldPtr->clearManifold(); -#endif //USE_PERSISTENT_CONTACTS +#endif //USE_PERSISTENT_CONTACTS btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = BT_LARGE_FLOAT; input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); - btBoxBoxDetector detector(box0,box1); - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + 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 @@ -73,11 +70,10 @@ void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrappe { resultOut->refreshContactPoints(); } -#endif //USE_PERSISTENT_CONTACTS - +#endif //USE_PERSISTENT_CONTACTS } -btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +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 index 59808df5a9d..eb210657659 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -26,22 +26,22 @@ class btPersistentManifold; ///box-box collision detection class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btBoxBoxCollisionAlgorithm(); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -49,18 +49,15 @@ public: } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { int bbsize = sizeof(btBoxBoxCollisionAlgorithm); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + return new (ptr) btBoxBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H - +#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 7043bde34f5..202039956e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -24,14 +24,12 @@ subject to the following restrictions: #include <float.h> #include <string.h> -btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2) -: m_box1(box1), -m_box2(box2) +btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1, const 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. @@ -48,67 +46,66 @@ m_box2(box2) // 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 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)); \ -} +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 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) +#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]; +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) +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; - } + 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]). @@ -117,60 +114,66 @@ void dLineClosestApproach (const btVector3& pa, const btVector3& ua, // 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]) +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; - } + // 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; + } } - pq += 2; - } - q = r; - r = (q==ret) ? buffer : ret; - nq = nr; - } - } - done: - if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); - return 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 @@ -181,538 +184,584 @@ static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) // 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[]) +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]; - if (btFabs(a+q) > SIMD_EPSILON) + // compute the centroid of the polygon in cx,cy + int i, j; + btScalar a, cx, cy, q; + if (n == 1) { - a = 1.f/(btScalar(3.0)*(a+q)); - } else + cx = p[0]; + cy = p[1]; + } + else if (n == 2) { - a=BT_LARGE_FLOAT; + cx = btScalar(0.5) * (p[0] + p[2]); + cy = btScalar(0.5) * (p[1] + p[3]); } - 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; - - *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 - - 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; + 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]; + if (btFabs(a + q) > SIMD_EPSILON) + { + a = 1.f / (btScalar(3.0) * (a + q)); + } + else + { + a = BT_LARGE_FLOAT; + } + cx = a * (cx + q * (p[n * 2 - 2] + p[0])); + cy = a * (cy + q * (p[n * 2 - 1] + p[1])); } - } - } -#if defined(DEBUG) || defined (_DEBUG) - btAssert (*iret != i0); // ensure iret got set + + // 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; + + *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 + + 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++; - } + 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(0.f, 0.f, 0.f); + 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; -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(0.f,0.f,0.f); - 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. + // 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 > SIMD_EPSILON) return 0; \ - l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > SIMD_EPSILON) { \ - 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); \ - } \ - } - - btScalar fudge2 (1.0e-5f); - - Q11 += fudge2; - Q12 += fudge2; - Q13 += fudge2; - - Q21 += fudge2; - Q22 += fudge2; - Q23 += fudge2; - - Q31 += fudge2; - Q32 += fudge2; - Q33 += fudge2; - - // 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); +#define TST(expr1, expr2, n1, n2, n3, cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > SIMD_EPSILON) return 0; \ + l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \ + if (l > SIMD_EPSILON) \ + { \ + 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); \ + } \ + } + + btScalar fudge2(1.0e-5f); + + Q11 += fudge2; + Q12 += fudge2; + Q13 += fudge2; + + Q21 += fudge2; + Q22 += fudge2; + Q23 += fudge2; + + Q31 += fudge2; + Q32 += fudge2; + Q33 += fudge2; + + // 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; + 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 { - - //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); - //contact[0].depth = *depth; - btVector3 pointInWorld; + 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); + 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); + output.addContactPoint(-normal, pb, -*depth); -#endif // - *return_code = code; +#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]; } - 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) { - - if (code<4) - { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) + 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]) { - 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 less contacts than we need, so we use them all - for (j=0; j < cnum; j++) + if (anr[1] > anr[2]) { - btVector3 pointInWorld; - for (i=0; i<3; i++) - pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j]; + 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) + { + if (code < 4) + { + // we have less contacts than we need, so we use them all + for (j = 0; j < cnum; 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 less contacts than we need, so we use them all + for (j = 0; j < cnum; j++) + { + btVector3 pointInWorld; + for (i = 0; i < 3; i++) + pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j]; //pointInWorld[i] = point[j*3+i] + pa[i]; - output.addContactPoint(-normal,pointInWorld,-dep[j]); + 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]; - if (code<4) - { - output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); - } else + } + 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++) { - output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); + if (dep[i] > maxdepth) + { + maxdepth = dep[i]; + i1 = i; + } } - } - cnum = maxc; - } - *return_code = code; - return cnum; + 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]; + if (code < 4) + { + output.addContactPoint(-normal, posInWorld, -dep[iret[j]]); + } + else + { + output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]); + } + } + cnum = maxc; + } + + *return_code = code; + return cnum; } -void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) +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; + dContactGeom* contact = 0; dMatrix3 R1; dMatrix3 R2; - for (int j=0;j<3;j++) + 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[0 + 4 * j] = transformA.getBasis()[j].x(); + R2[0 + 4 * j] = transformB.getBasis()[j].x(); - R1[2+4*j] = transformA.getBasis()[j].z(); - R2[2+4*j] = transformB.getBasis()[j].z(); + 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 - ); - + 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 index 39243777051..9f7d988fc11 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -19,11 +19,9 @@ subject to the following restrictions: #ifndef BT_BOX_BOX_DETECTOR_H #define BT_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 @@ -32,13 +30,11 @@ struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface const btBoxShape* m_box2; public: + btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2); - btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2); - - virtual ~btBoxBoxDetector() {}; - - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual ~btBoxBoxDetector(){}; + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); }; -#endif //BT_BOX_BOX_DETECTOR_H +#endif //BT_BOX_BOX_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 66949849448..d6e15f555bf 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -23,11 +23,9 @@ 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 +class btCollisionConfiguration { - public: - virtual ~btCollisionConfiguration() { } @@ -37,10 +35,9 @@ public: virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; - + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; }; -#endif //BT_COLLISION_CONFIGURATION - +#endif //BT_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index 62ee66c4e9a..bd812849393 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -26,20 +26,18 @@ struct btCollisionAlgorithmConstructionInfo; struct btCollisionAlgorithmCreateFunc { bool m_swapped; - + btCollisionAlgorithmCreateFunc() - :m_swapped(false) + : m_swapped(false) { } virtual ~btCollisionAlgorithmCreateFunc(){}; - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo&, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - (void)body0Wrap; (void)body1Wrap; return 0; } }; -#endif //BT_COLLISION_CREATE_FUNC - +#endif //BT_COLLISION_CREATE_FUNC diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 3b6913c0e1e..25b2b1ea464 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -13,10 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btCollisionDispatcher.h" - +#include "LinearMath/btQuickprof.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" @@ -27,82 +25,73 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -int gNumManifold = 0; - #ifdef BT_DEBUG #include <stdio.h> #endif - -btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): -m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), - m_collisionConfiguration(collisionConfiguration) +btCollisionDispatcher::btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration) : m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), + m_collisionConfiguration(collisionConfiguration) { int i; setNearCallback(defaultNearCallback); - + m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); - for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) + for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++) { - m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); - btAssert(m_doubleDispatch[i][j]); + m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i, j); + btAssert(m_doubleDispatchContactPoints[i][j]); + m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j); } } - - } +void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) +{ + m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc; +} -void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) { - m_doubleDispatch[proxyType0][proxyType1] = createFunc; + m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc; } btCollisionDispatcher::~btCollisionDispatcher() { } -btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) -{ - gNumManifold++; - +btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) +{ //btAssert(gNumManifold < 65535); - - //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) - - btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? - btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) - : gContactBreakingThreshold ; - - btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); - - void* mem = 0; - - if (m_persistentManifoldPoolAllocator->getFreeCount()) - { - mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); - } else + + btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) + : gContactBreakingThreshold; + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold()); + + void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); + if (NULL == mem) { //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. - if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0) + if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0) { - mem = btAlignedAlloc(sizeof(btPersistentManifold),16); - } else + mem = btAlignedAlloc(sizeof(btPersistentManifold), 16); + } + else { btAssert(0); //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration return 0; } } - btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); + btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); @@ -114,18 +103,14 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) manifold->clearManifold(); } - void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) { - - gNumManifold--; - //printf("releaseManifold: gNumManifold %d\n",gNumManifold); clearManifold(manifold); int findIndex = manifold->m_index1a; btAssert(findIndex < m_manifoldsPtr.size()); - m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); + m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1); m_manifoldsPtr[findIndex]->m_index1a = findIndex; m_manifoldsPtr.pop_back(); @@ -133,42 +118,44 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) if (m_persistentManifoldPoolAllocator->validPtr(manifold)) { m_persistentManifoldPoolAllocator->freeMemory(manifold); - } else + } + else { btAlignedFree(manifold); } - } - - -btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold) +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) { - btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = this; ci.m_manifold = sharedManifold; - btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap); + btCollisionAlgorithm* algo = 0; + if (algoType == BT_CONTACT_POINT_ALGORITHMS) + { + algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } + else + { + algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } return algo; } - - - -bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1) +bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) { //here you can do filtering - bool hasResponse = + bool hasResponse = (body0->hasContactResponse() && body1->hasContactResponse()); //no response between two static/kinematic bodies: hasResponse = hasResponse && - ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject())); + ((!body0->isStaticOrKinematicObject()) || (!body1->isStaticOrKinematicObject())); return hasResponse; } -bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) +bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) { btAssert(body0); btAssert(body1); @@ -185,31 +172,27 @@ bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); } } -#endif //BT_DEBUG +#endif //BT_DEBUG if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0))) needsCollision = false; - - return needsCollision ; + return needsCollision; } - - ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) ///this is useful for the collision dispatcher. class btCollisionPairCallback : public btOverlapCallback { const btDispatcherInfo& m_dispatchInfo; - btCollisionDispatcher* m_dispatcher; + btCollisionDispatcher* m_dispatcher; public: - - btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) - :m_dispatchInfo(dispatchInfo), - m_dispatcher(dispatcher) + btCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher) + : m_dispatchInfo(dispatchInfo), + m_dispatcher(dispatcher) { } @@ -221,85 +204,76 @@ public: } */ - virtual ~btCollisionPairCallback() {} - - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { - (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); - + (*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo); return false; } }; - - -void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) +void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) { //m_blockedForChanges = true; - btCollisionPairCallback collisionCallback(dispatchInfo,this); + btCollisionPairCallback collisionCallback(dispatchInfo, this); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + { + BT_PROFILE("processAllOverlappingPairs"); + pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo); + } //m_blockedForChanges = false; - } - - - //by default, Bullet will use this near callback 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; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + + if (dispatcher.needsCollision(colObj0, colObj1)) + { + btCollisionObjectWrapper obj0Wrap(0, colObj0->getCollisionShape(), colObj0, colObj0->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obj1Wrap(0, colObj1->getCollisionShape(), colObj1, colObj1->getWorldTransform(), -1, -1); - if (dispatcher.needsCollision(colObj0,colObj1)) + //dispatcher will keep algorithms persistent in the collision pair + if (!collisionPair.m_algorithm) { - btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1); - btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1); + collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS); + } + if (collisionPair.m_algorithm) + { + btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap); - //dispatcher will keep algorithms persistent in the collision pair - if (!collisionPair.m_algorithm) + if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) { - collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap); - } + //discrete collision detection query - if (collisionPair.m_algorithm) + collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult); + } + else { - btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap); - - if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) - { - //discrete collision detection query - - collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult); - } else - { - //continuous collision detection query, time of impact (toi) - btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - - } + //continuous collision detection query, time of impact (toi) + btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; } } - + } } - void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) { - if (m_collisionAlgorithmPoolAllocator->getFreeCount()) + void* mem = m_collisionAlgorithmPoolAllocator->allocate(size); + if (NULL == mem) { - return m_collisionAlgorithmPoolAllocator->allocate(size); + //warn user for overflow? + return btAlignedAlloc(static_cast<size_t>(size), 16); } - - //warn user for overflow? - return btAlignedAlloc(static_cast<size_t>(size), 16); + return mem; } void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) @@ -307,7 +281,8 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) { m_collisionAlgorithmPoolAllocator->freeMemory(ptr); - } else + } + else { btAlignedFree(ptr); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 92696ee5429..04309670cfc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -37,33 +37,28 @@ class btCollisionDispatcher; ///user can override this nearcallback for collision filtering and more finegrained control over collision detection typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); - ///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. ///Time of Impact, Closest Points and Penetration Depth. class btCollisionDispatcher : public btDispatcher { - protected: + int m_dispatcherFlags; - int m_dispatcherFlags; + btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; - btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; + btNearCallback m_nearCallback; - btManifoldResult m_defaultManifoldResult; + btPoolAllocator* m_collisionAlgorithmPoolAllocator; - btNearCallback m_nearCallback; - - btPoolAllocator* m_collisionAlgorithmPoolAllocator; + btPoolAllocator* m_persistentManifoldPoolAllocator; - btPoolAllocator* m_persistentManifoldPoolAllocator; + btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - - btCollisionConfiguration* m_collisionConfiguration; + btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + btCollisionConfiguration* m_collisionConfiguration; public: - enum DispatcherFlags { CD_STATIC_STATIC_REPORTED = 1, @@ -71,101 +66,104 @@ public: CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 }; - int getDispatcherFlags() const + int getDispatcherFlags() const { return m_dispatcherFlags; } - void setDispatcherFlags(int flags) + void setDispatcherFlags(int flags) { m_dispatcherFlags = flags; } ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions - void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + void registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); - int getNumManifolds() const - { - return int( m_manifoldsPtr.size()); + void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + + int getNumManifolds() const + { + return int(m_manifoldsPtr.size()); } - btPersistentManifold** getInternalManifoldPointer() + btPersistentManifold** getInternalManifoldPointer() { - return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0; + return m_manifoldsPtr.size() ? &m_manifoldsPtr[0] : 0; } - btPersistentManifold* getManifoldByIndexInternal(int index) + btPersistentManifold* getManifoldByIndexInternal(int index) { + btAssert(index>=0); + btAssert(index<m_manifoldsPtr.size()); return m_manifoldsPtr[index]; } - const btPersistentManifold* getManifoldByIndexInternal(int index) const + const btPersistentManifold* getManifoldByIndexInternal(int index) const { + btAssert(index>=0); + btAssert(index<m_manifoldsPtr.size()); return m_manifoldsPtr[index]; } - btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration); + btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionDispatcher(); - virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1); - - virtual void releaseManifold(btPersistentManifold* manifold); + virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1); + virtual void releaseManifold(btPersistentManifold* manifold); virtual void clearManifold(btPersistentManifold* manifold); - btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0); - - virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1); - - virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1); - - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ; + btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType); + + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1); + + virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1); - void setNearCallback(btNearCallback nearCallback) + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher); + + void setNearCallback(btNearCallback nearCallback) { - m_nearCallback = nearCallback; + m_nearCallback = nearCallback; } - btNearCallback getNearCallback() const + btNearCallback getNearCallback() const { return m_nearCallback; } //by default, Bullet will use this near callback - static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); + static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); - virtual void* allocateCollisionAlgorithm(int size); + virtual void* allocateCollisionAlgorithm(int size); - virtual void freeCollisionAlgorithm(void* ptr); + virtual void freeCollisionAlgorithm(void* ptr); - btCollisionConfiguration* getCollisionConfiguration() + btCollisionConfiguration* getCollisionConfiguration() { return m_collisionConfiguration; } - const btCollisionConfiguration* getCollisionConfiguration() const + const btCollisionConfiguration* getCollisionConfiguration() const { return m_collisionConfiguration; } - void setCollisionConfiguration(btCollisionConfiguration* config) + void setCollisionConfiguration(btCollisionConfiguration* config) { m_collisionConfiguration = config; } - virtual btPoolAllocator* getInternalManifoldPool() + virtual btPoolAllocator* getInternalManifoldPool() { return m_persistentManifoldPoolAllocator; } - virtual const btPoolAllocator* getInternalManifoldPool() const + virtual const btPoolAllocator* getInternalManifoldPool() const { return m_persistentManifoldPoolAllocator; } - }; -#endif //BT_COLLISION__DISPATCHER_H - +#endif //BT_COLLISION__DISPATCHER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp new file mode 100644 index 00000000000..89bc8d920ef --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp @@ -0,0 +1,162 @@ +/* +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 "btCollisionDispatcherMt.h" +#include "LinearMath/btQuickprof.h" + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.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" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize) + : btCollisionDispatcher(config) +{ + m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads()); + m_batchUpdating = false; + m_grainSize = grainSize; // iterations per task +} + +btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) +{ + //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) + + btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) + : gContactBreakingThreshold; + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold()); + + void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); + if (NULL == mem) + { + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0) + { + mem = btAlignedAlloc(sizeof(btPersistentManifold), 16); + } + else + { + btAssert(0); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } + } + btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold); + if (!m_batchUpdating) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + //btAssert( !btThreadsAreRunning() ); + manifold->m_index1a = m_manifoldsPtr.size(); + m_manifoldsPtr.push_back(manifold); + } + else + { + m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold); + } + + return manifold; +} + +void btCollisionDispatcherMt::releaseManifold(btPersistentManifold* manifold) +{ + clearManifold(manifold); + //btAssert( !btThreadsAreRunning() ); + if (!m_batchUpdating) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + 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_persistentManifoldPoolAllocator->freeMemory(manifold); + } + else + { + btAlignedFree(manifold); + } +} + +struct CollisionDispatcherUpdater : public btIParallelForBody +{ + btBroadphasePair* mPairArray; + btNearCallback mCallback; + btCollisionDispatcher* mDispatcher; + const btDispatcherInfo* mInfo; + + CollisionDispatcherUpdater() + { + mPairArray = NULL; + mCallback = NULL; + mDispatcher = NULL; + mInfo = NULL; + } + void forLoop(int iBegin, int iEnd) const + { + for (int i = iBegin; i < iEnd; ++i) + { + btBroadphasePair* pair = &mPairArray[i]; + mCallback(*pair, *mDispatcher, *mInfo); + } + } +}; + +void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) +{ + const int pairCount = pairCache->getNumOverlappingPairs(); + if (pairCount == 0) + { + return; + } + CollisionDispatcherUpdater updater; + updater.mCallback = getNearCallback(); + updater.mPairArray = pairCache->getOverlappingPairArrayPtr(); + updater.mDispatcher = this; + updater.mInfo = &info; + + m_batchUpdating = true; + btParallelFor(0, pairCount, m_grainSize, updater); + m_batchUpdating = false; + + // merge new manifolds, if any + for (int i = 0; i < m_batchManifoldsPtr.size(); ++i) + { + btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i]; + + for (int j = 0; j < batchManifoldsPtr.size(); ++j) + { + m_manifoldsPtr.push_back(batchManifoldsPtr[j]); + } + + batchManifoldsPtr.resizeNoInitialize(0); + } + + // update the indices (used when releasing manifolds) + for (int i = 0; i < m_manifoldsPtr.size(); ++i) + { + m_manifoldsPtr[i]->m_index1a = i; + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h new file mode 100644 index 00000000000..1155de2cfe6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h @@ -0,0 +1,38 @@ +/* +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_DISPATCHER_MT_H +#define BT_COLLISION_DISPATCHER_MT_H + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "LinearMath/btThreads.h" + +class btCollisionDispatcherMt : public btCollisionDispatcher +{ +public: + btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize = 40); + + virtual btPersistentManifold* getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) BT_OVERRIDE; + virtual void releaseManifold(btPersistentManifold* manifold) BT_OVERRIDE; + + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE; + +protected: + btAlignedObjectArray<btAlignedObjectArray<btPersistentManifold*> > m_batchManifoldsPtr; + bool m_batchUpdating; + int m_grainSize; +}; + +#endif //BT_COLLISION_DISPATCHER_MT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 395df3a550f..b48d9301d74 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -13,36 +13,45 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btCollisionObject.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" btCollisionObject::btCollisionObject() - : m_anisotropicFriction(1.f,1.f,1.f), - m_hasAnisotropicFriction(false), - m_contactProcessingThreshold(BT_LARGE_FLOAT), - m_broadphaseHandle(0), - m_collisionShape(0), - m_extensionPointer(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_rollingFriction(0.0f), - m_internalType(CO_COLLISION_OBJECT), - m_userObjectPointer(0), - m_userIndex(-1), - m_hitFraction(btScalar(1.)), - m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdMotionThreshold(btScalar(0.)), - m_checkCollideWith(false), - m_updateRevision(0) + : m_interpolationLinearVelocity(0.f, 0.f, 0.f), + m_interpolationAngularVelocity(0.f, 0.f, 0.f), + m_anisotropicFriction(1.f, 1.f, 1.f), + m_hasAnisotropicFriction(false), + m_contactProcessingThreshold(BT_LARGE_FLOAT), + m_broadphaseHandle(0), + m_collisionShape(0), + m_extensionPointer(0), + m_rootCollisionShape(0), + m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), + m_islandTag1(-1), + m_companionId(-1), + m_worldArrayIndex(-1), + m_activationState1(1), + m_deactivationTime(btScalar(0.)), + m_friction(btScalar(0.5)), + m_restitution(btScalar(0.)), + m_rollingFriction(0.0f), + m_spinningFriction(0.f), + m_contactDamping(.1), + m_contactStiffness(BT_LARGE_FLOAT), + m_internalType(CO_COLLISION_OBJECT), + m_userObjectPointer(0), + m_userIndex2(-1), + m_userIndex(-1), + m_userIndex3(-1), + m_hitFraction(btScalar(1.)), + m_ccdSweptSphereRadius(btScalar(0.)), + m_ccdMotionThreshold(btScalar(0.)), + m_checkCollideWith(false), + m_updateRevision(0) { m_worldTransform.setIdentity(); + m_interpolationWorldTransform.setIdentity(); } btCollisionObject::~btCollisionObject() @@ -50,8 +59,8 @@ btCollisionObject::~btCollisionObject() } void btCollisionObject::setActivationState(int newState) const -{ - if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) +{ + if ((m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) m_activationState1 = newState; } @@ -62,7 +71,7 @@ void btCollisionObject::forceActivationState(int newState) const void btCollisionObject::activate(bool forceActivation) const { - if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) + if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT))) { setActivationState(ACTIVE_TAG); m_deactivationTime = btScalar(0.); @@ -71,7 +80,6 @@ void btCollisionObject::activate(bool forceActivation) const const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const { - btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer; m_worldTransform.serialize(dataOut->m_worldTransform); @@ -83,7 +91,7 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold; dataOut->m_broadphaseHandle = 0; dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape); - dataOut->m_rootCollisionShape = 0;//@todo + dataOut->m_rootCollisionShape = 0; //@todo dataOut->m_collisionFlags = m_collisionFlags; dataOut->m_islandTag1 = m_islandTag1; dataOut->m_companionId = m_companionId; @@ -91,10 +99,12 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_deactivationTime = m_deactivationTime; dataOut->m_friction = m_friction; dataOut->m_rollingFriction = m_rollingFriction; + dataOut->m_contactDamping = m_contactDamping; + dataOut->m_contactStiffness = m_contactStiffness; dataOut->m_restitution = m_restitution; dataOut->m_internalType = m_internalType; - - char* name = (char*) serializer->findNameForPointer(this); + + char* name = (char*)serializer->findNameForPointer(this); dataOut->m_name = (char*)serializer->getUniquePointer(name); if (dataOut->m_name) { @@ -104,15 +114,25 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; dataOut->m_checkCollideWith = m_checkCollideWith; - + if (m_broadphaseHandle) + { + dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup; + dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask; + dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId; + } + else + { + dataOut->m_collisionFilterGroup = 0; + dataOut->m_collisionFilterMask = 0; + dataOut->m_uniqueId = -1; + } return btCollisionObjectDataName; } - void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const { int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_COLLISIONOBJECT_CODE, (void*)this); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c68402418f7..3a1c271af5b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -25,8 +25,8 @@ subject to the following restrictions: #define DISABLE_DEACTIVATION 4 #define DISABLE_SIMULATION 5 -struct btBroadphaseProxy; -class btCollisionShape; +struct btBroadphaseProxy; +class btCollisionShape; struct btCollisionShapeData; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" @@ -42,110 +42,120 @@ typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; #define btCollisionObjectDataName "btCollisionObjectFloatData" #endif - -/// btCollisionObject can be used to manage collision detection objects. +/// btCollisionObject can be used to manage collision detection objects. /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. /// They can be added to the btCollisionWorld. -ATTRIBUTE_ALIGNED16(class) btCollisionObject +ATTRIBUTE_ALIGNED16(class) +btCollisionObject { - protected: - - btTransform m_worldTransform; + btTransform m_worldTransform; ///m_interpolationWorldTransform is used for CCD and interpolation ///it can be either previous or future (predicted) transform - btTransform m_interpolationWorldTransform; - //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) + btTransform m_interpolationWorldTransform; + //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) //without destroying the continuous interpolated motion (which uses this interpolation velocities) - btVector3 m_interpolationLinearVelocity; - btVector3 m_interpolationAngularVelocity; - - btVector3 m_anisotropicFriction; - int m_hasAnisotropicFriction; - btScalar m_contactProcessingThreshold; - - btBroadphaseProxy* m_broadphaseHandle; - btCollisionShape* m_collisionShape; + btVector3 m_interpolationLinearVelocity; + btVector3 m_interpolationAngularVelocity; + + btVector3 m_anisotropicFriction; + int m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; + + btBroadphaseProxy* m_broadphaseHandle; + btCollisionShape* m_collisionShape; ///m_extensionPointer is used by some internal low-level Bullet extensions. - void* m_extensionPointer; - + void* m_extensionPointer; + ///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; + btCollisionShape* m_rootCollisionShape; - int m_collisionFlags; + int m_collisionFlags; - int m_islandTag1; - int m_companionId; + int m_islandTag1; + int m_companionId; + int m_worldArrayIndex; // index of object in world's collisionObjects array - mutable int m_activationState1; - mutable btScalar m_deactivationTime; + mutable int m_activationState1; + mutable btScalar m_deactivationTime; - btScalar m_friction; - btScalar m_restitution; - btScalar m_rollingFriction; + btScalar m_friction; + btScalar m_restitution; + btScalar m_rollingFriction; //torsional friction orthogonal to contact normal (useful to stop spheres rolling forever) + btScalar m_spinningFriction; // torsional friction around the contact normal (useful for grasping) + btScalar m_contactDamping; + btScalar m_contactStiffness; ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///do not assign your own m_internalType unless you write a new dynamics object class. - int m_internalType; + int m_internalType; ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - - int m_userIndex; + void* m_userObjectPointer; + + int m_userIndex2; + + int m_userIndex; + + int m_userIndex3; ///time of impact calculation - btScalar m_hitFraction; - + btScalar m_hitFraction; + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar m_ccdSweptSphereRadius; + btScalar m_ccdSweptSphereRadius; /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold - btScalar m_ccdMotionThreshold; - + btScalar m_ccdMotionThreshold; + /// If some object should have elaborate collision filtering by sub-classes - int m_checkCollideWith; + int m_checkCollideWith; btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck; ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. - int m_updateRevision; + int m_updateRevision; + btVector3 m_customDebugColorRGB; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); enum CollisionFlags { - CF_STATIC_OBJECT= 1, - CF_KINEMATIC_OBJECT= 2, + CF_STATIC_OBJECT = 1, + CF_KINEMATIC_OBJECT = 2, CF_NO_CONTACT_RESPONSE = 4, - CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + CF_CUSTOM_MATERIAL_CALLBACK = 8, //this allows per-triangle material (friction/restitution) CF_CHARACTER_OBJECT = 16, - CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing - CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing + CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing + CF_DISABLE_SPU_COLLISION_PROCESSING = 64, //disable parallel/SPU processing + CF_HAS_CONTACT_STIFFNESS_DAMPING = 128, + CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256, + CF_HAS_FRICTION_ANCHOR = 512, + CF_HAS_COLLISION_SOUND_TRIGGER = 1024 }; - enum CollisionObjectTypes + enum CollisionObjectTypes { - CO_COLLISION_OBJECT =1, - CO_RIGID_BODY=2, + CO_COLLISION_OBJECT = 1, + CO_RIGID_BODY = 2, ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter ///It is useful for collision sensors, explosion objects, character controller etc. - CO_GHOST_OBJECT=4, - CO_SOFT_BODY=8, - CO_HF_FLUID=16, - CO_USER_TYPE=32, - CO_FEATHERSTONE_LINK=64 + CO_GHOST_OBJECT = 4, + CO_SOFT_BODY = 8, + CO_HF_FLUID = 16, + CO_USER_TYPE = 32, + CO_FEATHERSTONE_LINK = 64 }; enum AnisotropicFrictionFlags { - CF_ANISOTROPIC_FRICTION_DISABLED=0, + CF_ANISOTROPIC_FRICTION_DISABLED = 0, CF_ANISOTROPIC_FRICTION = 1, CF_ANISOTROPIC_ROLLING_FRICTION = 2 }; @@ -153,76 +163,77 @@ public: 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); + 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, int frictionMode = CF_ANISOTROPIC_FRICTION) + void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION) { m_anisotropicFriction = anisotropicFriction; - bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f); - m_hasAnisotropicFriction = isUnity?frictionMode : 0; + bool isUnity = (anisotropicFriction[0] != 1.f) || (anisotropicFriction[1] != 1.f) || (anisotropicFriction[2] != 1.f); + m_hasAnisotropicFriction = isUnity ? frictionMode : 0; } - bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const + bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const { - return (m_hasAnisotropicFriction&frictionMode)!=0; + return (m_hasAnisotropicFriction & frictionMode) != 0; } ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges - void setContactProcessingThreshold( btScalar contactProcessingThreshold) + void setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; } - btScalar getContactProcessingThreshold() const + btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; } - SIMD_FORCE_INLINE bool isStaticObject() const { + SIMD_FORCE_INLINE bool isStaticObject() const + { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; } - SIMD_FORCE_INLINE bool isKinematicObject() const + SIMD_FORCE_INLINE bool isKinematicObject() const { return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; } - SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const + SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const { - return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ; + return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0; } - SIMD_FORCE_INLINE bool hasContactResponse() const { - return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0; + SIMD_FORCE_INLINE bool hasContactResponse() const + { + return (m_collisionFlags & CF_NO_CONTACT_RESPONSE) == 0; } - btCollisionObject(); virtual ~btCollisionObject(); - virtual void setCollisionShape(btCollisionShape* collisionShape) + virtual void setCollisionShape(btCollisionShape * collisionShape) { m_updateRevision++; m_collisionShape = collisionShape; m_rootCollisionShape = collisionShape; } - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; } - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { return m_collisionShape; } - void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck) + void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck) { if (ignoreCollisionCheck) { @@ -240,7 +251,17 @@ public: m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; } - virtual bool checkCollideWithOverride(const btCollisionObject* co) const + int getNumObjectsWithoutCollision() const + { + return m_objectsWithoutCollisionCheck.size(); + } + + const btCollisionObject* getObjectWithoutCollision(int index) + { + return m_objectsWithoutCollisionCheck[index]; + } + + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); if (index < m_objectsWithoutCollisionCheck.size()) @@ -250,249 +271,325 @@ public: return true; } - - - - ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. + ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. - void* internalGetExtensionPointer() const + void* internalGetExtensionPointer() const { return m_extensionPointer; } ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. - void internalSetExtensionPointer(void* pointer) + void internalSetExtensionPointer(void* pointer) { m_extensionPointer = pointer; } - SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} - + SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1; } + void setActivationState(int newState) const; - void setDeactivationTime(btScalar time) + void setDeactivationTime(btScalar time) { m_deactivationTime = time; } - btScalar getDeactivationTime() const + btScalar getDeactivationTime() const { return m_deactivationTime; } void forceActivationState(int newState) const; - void activate(bool forceActivation = false) const; + void activate(bool forceActivation = false) const; SIMD_FORCE_INLINE bool isActive() const { return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } - void setRestitution(btScalar rest) + void setRestitution(btScalar rest) { m_updateRevision++; m_restitution = rest; } - btScalar getRestitution() const + btScalar getRestitution() const { return m_restitution; } - void setFriction(btScalar frict) + void setFriction(btScalar frict) { m_updateRevision++; m_friction = frict; } - btScalar getFriction() const + btScalar getFriction() const { return m_friction; } - void setRollingFriction(btScalar frict) + void setRollingFriction(btScalar frict) { m_updateRevision++; m_rollingFriction = frict; } - btScalar getRollingFriction() const + btScalar getRollingFriction() const { return m_rollingFriction; } + void setSpinningFriction(btScalar frict) + { + m_updateRevision++; + m_spinningFriction = frict; + } + btScalar getSpinningFriction() const + { + return m_spinningFriction; + } + void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping) + { + m_updateRevision++; + m_contactStiffness = stiffness; + m_contactDamping = damping; + m_collisionFlags |= CF_HAS_CONTACT_STIFFNESS_DAMPING; + + //avoid divisions by zero... + if (m_contactStiffness < SIMD_EPSILON) + { + m_contactStiffness = SIMD_EPSILON; + } + } + + btScalar getContactStiffness() const + { + return m_contactStiffness; + } + + btScalar getContactDamping() const + { + return m_contactDamping; + } ///reserved for Bullet internal usage - int getInternalType() const + int getInternalType() const { return m_internalType; } - btTransform& getWorldTransform() + btTransform& getWorldTransform() { return m_worldTransform; } - const btTransform& getWorldTransform() const + const btTransform& getWorldTransform() const { return m_worldTransform; } - void setWorldTransform(const btTransform& worldTrans) + void setWorldTransform(const btTransform& worldTrans) { m_updateRevision++; m_worldTransform = worldTrans; } - - SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() + SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() { return m_broadphaseHandle; } - SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const + SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const { return m_broadphaseHandle; } - void setBroadphaseHandle(btBroadphaseProxy* handle) + void setBroadphaseHandle(btBroadphaseProxy * handle) { m_broadphaseHandle = handle; } - - const btTransform& getInterpolationWorldTransform() const + const btTransform& getInterpolationWorldTransform() const { return m_interpolationWorldTransform; } - btTransform& getInterpolationWorldTransform() + btTransform& getInterpolationWorldTransform() { return m_interpolationWorldTransform; } - void setInterpolationWorldTransform(const btTransform& trans) + void setInterpolationWorldTransform(const btTransform& trans) { m_updateRevision++; m_interpolationWorldTransform = trans; } - void setInterpolationLinearVelocity(const btVector3& linvel) + void setInterpolationLinearVelocity(const btVector3& linvel) { m_updateRevision++; m_interpolationLinearVelocity = linvel; } - void setInterpolationAngularVelocity(const btVector3& angvel) + void setInterpolationAngularVelocity(const btVector3& angvel) { m_updateRevision++; m_interpolationAngularVelocity = angvel; } - const btVector3& getInterpolationLinearVelocity() const + const btVector3& getInterpolationLinearVelocity() const { return m_interpolationLinearVelocity; } - const btVector3& getInterpolationAngularVelocity() const + const btVector3& getInterpolationAngularVelocity() const { return m_interpolationAngularVelocity; } SIMD_FORCE_INLINE int getIslandTag() const { - return m_islandTag1; + return m_islandTag1; } - void setIslandTag(int tag) + void setIslandTag(int tag) { m_islandTag1 = tag; } SIMD_FORCE_INLINE int getCompanionId() const { - return m_companionId; + return m_companionId; } - void setCompanionId(int id) + void setCompanionId(int id) { m_companionId = id; } - SIMD_FORCE_INLINE btScalar getHitFraction() const + SIMD_FORCE_INLINE int getWorldArrayIndex() const { - return m_hitFraction; + return m_worldArrayIndex; } - void setHitFraction(btScalar hitFraction) + // only should be called by CollisionWorld + void setWorldArrayIndex(int ix) + { + m_worldArrayIndex = ix; + } + + SIMD_FORCE_INLINE btScalar getHitFraction() const + { + return m_hitFraction; + } + + void setHitFraction(btScalar hitFraction) { m_hitFraction = hitFraction; } - - SIMD_FORCE_INLINE int getCollisionFlags() const + SIMD_FORCE_INLINE int getCollisionFlags() const { return m_collisionFlags; } - void setCollisionFlags(int flags) + void setCollisionFlags(int flags) { m_collisionFlags = flags; } - + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar getCcdSweptSphereRadius() const + btScalar getCcdSweptSphereRadius() const { return m_ccdSweptSphereRadius; } ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - void setCcdSweptSphereRadius(btScalar radius) + void setCcdSweptSphereRadius(btScalar radius) { m_ccdSweptSphereRadius = radius; } - btScalar getCcdMotionThreshold() const + btScalar getCcdMotionThreshold() const { return m_ccdMotionThreshold; } - btScalar getCcdSquareMotionThreshold() const + btScalar getCcdSquareMotionThreshold() const { - return m_ccdMotionThreshold*m_ccdMotionThreshold; + return m_ccdMotionThreshold * m_ccdMotionThreshold; } - - /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold - void setCcdMotionThreshold(btScalar ccdMotionThreshold) + void setCcdMotionThreshold(btScalar ccdMotionThreshold) { m_ccdMotionThreshold = ccdMotionThreshold; } ///users can point to their objects, userPointer is not used by Bullet - void* getUserPointer() const + void* getUserPointer() const { return m_userObjectPointer; } - int getUserIndex() const + int getUserIndex() const { return m_userIndex; } + + int getUserIndex2() const + { + return m_userIndex2; + } + + int getUserIndex3() const + { + return m_userIndex3; + } + ///users can point to their objects, userPointer is not used by Bullet - void setUserPointer(void* userPointer) + void setUserPointer(void* userPointer) { m_userObjectPointer = userPointer; } ///users can point to their objects, userPointer is not used by Bullet - void setUserIndex(int index) + void setUserIndex(int index) { m_userIndex = index; } - int getUpdateRevisionInternal() const + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + + void setUserIndex3(int index) + { + m_userIndex3 = index; + } + + int getUpdateRevisionInternal() const { return m_updateRevision; } + void setCustomDebugColor(const btVector3& colorRGB) + { + m_customDebugColorRGB = colorRGB; + m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + void removeCustomDebugColor() + { + m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + bool getCustomDebugColor(btVector3 & colorRGB) const + { + bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + if (hasCustomColor) + { + colorRGB = m_customDebugColorRGB; + } + return hasCustomColor; + } inline bool checkCollideWith(const btCollisionObject* co) const { @@ -502,15 +599,16 @@ public: return true; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; - - virtual void serializeSingleObject(class btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + virtual void serializeSingleObject(class btSerializer * serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCollisionObjectDoubleData { @@ -528,11 +626,12 @@ struct btCollisionObjectDoubleData double m_deactivationTime; double m_friction; double m_rollingFriction; + double m_contactDamping; + double m_contactStiffness; double m_restitution; double m_hitFraction; double m_ccdSweptSphereRadius; double m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -540,8 +639,9 @@ struct btCollisionObjectDoubleData int m_activationState1; int m_internalType; int m_checkCollideWith; - - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 @@ -561,12 +661,12 @@ struct btCollisionObjectFloatData float m_deactivationTime; float m_friction; float m_rollingFriction; - + float m_contactDamping; + float m_contactStiffness; float m_restitution; float m_hitFraction; float m_ccdSweptSphereRadius; float m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -574,16 +674,15 @@ struct btCollisionObjectFloatData int m_activationState1; int m_internalType; int m_checkCollideWith; - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId; }; +// clang-format on - - -SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const { return sizeof(btCollisionObjectData); } - - -#endif //BT_COLLISION_OBJECT_H +#endif //BT_COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h index 952440b7dee..56341b7d292 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h @@ -1,25 +1,25 @@ #ifndef BT_COLLISION_OBJECT_WRAPPER_H #define BT_COLLISION_OBJECT_WRAPPER_H -///btCollisionObjectWrapperis an internal data structure. +///btCollisionObjectWrapperis an internal data structure. ///Most users can ignore this and use btCollisionObject and btCollisionShape instead class btCollisionShape; class btCollisionObject; class btTransform; -#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition +#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition #define BT_DECLARE_STACK_ONLY_OBJECT \ - private: \ - void* operator new(size_t size); \ - void operator delete(void*); +private: \ + void* operator new(size_t size); \ + void operator delete(void*); struct btCollisionObjectWrapper; struct btCollisionObjectWrapper { -BT_DECLARE_STACK_ONLY_OBJECT + BT_DECLARE_STACK_ONLY_OBJECT private: - btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. + btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&); public: @@ -27,17 +27,23 @@ public: const btCollisionShape* m_shape; const btCollisionObject* m_collisionObject; const btTransform& m_worldTransform; - int m_partId; - int m_index; + const btTransform* m_preTransform; + int m_partId; + int m_index; btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index) - : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), - m_partId(partId), m_index(index) - {} + : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(NULL), m_partId(partId), m_index(index) + { + } + + btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, const btTransform& preTransform, int partId, int index) + : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(&preTransform), m_partId(partId), m_index(index) + { + } SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; } SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; } SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; } }; -#endif //BT_COLLISION_OBJECT_WRAPPER_H +#endif //BT_COLLISION_OBJECT_WRAPPER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index c505ed5d508..71184f36acd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -19,8 +19,10 @@ subject to the following restrictions: #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/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -37,7 +39,6 @@ subject to the following restrictions: //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION - //#define USE_BRUTEFORCE_RAYBROADPHASE 1 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest //#define RECALCULATE_AABB_RAYCAST 1 @@ -47,7 +48,6 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" - ///for debug drawing //for debug rendering @@ -64,25 +64,21 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - - -btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) -:m_dispatcher1(dispatcher), -m_broadphasePairCache(pairCache), -m_debugDrawer(0), -m_forceUpdateAllAabbs(true) +btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) + : m_dispatcher1(dispatcher), + m_broadphasePairCache(pairCache), + m_debugDrawer(0), + m_forceUpdateAllAabbs(true) { } - btCollisionWorld::~btCollisionWorld() { - //clean up remaining objects int i; - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { - btCollisionObject* collisionObject= m_collisionObjects[i]; + btCollisionObject* collisionObject = m_collisionObjects[i]; btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) @@ -90,73 +86,83 @@ btCollisionWorld::~btCollisionWorld() // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1); + getBroadphase()->destroyProxy(bp, m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } - - } - - - - - - - - - -void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) +void btCollisionWorld::refreshBroadphaseProxy(btCollisionObject* collisionObject) { + if (collisionObject->getBroadphaseHandle()) + { + int collisionFilterGroup = collisionObject->getBroadphaseHandle()->m_collisionFilterGroup; + int collisionFilterMask = collisionObject->getBroadphaseHandle()->m_collisionFilterMask; + + getBroadphase()->destroyProxy(collisionObject->getBroadphaseHandle(), getDispatcher()); + + //calculate new AABB + btTransform trans = collisionObject->getWorldTransform(); + + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb); + + int type = collisionObject->getCollisionShape()->getShapeType(); + collisionObject->setBroadphaseHandle(getBroadphase()->createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask, + m_dispatcher1)); + } +} +void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) +{ btAssert(collisionObject); //check that the object isn't already added - btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert(m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world + collisionObject->setWorldArrayIndex(m_collisionObjects.size()); m_collisionObjects.push_back(collisionObject); //calculate new AABB btTransform trans = collisionObject->getWorldTransform(); - btVector3 minAabb; - btVector3 maxAabb; - collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb); int type = collisionObject->getCollisionShape()->getShapeType(); - collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( + collisionObject->setBroadphaseHandle(getBroadphase()->createProxy( minAabb, maxAabb, type, collisionObject, collisionFilterGroup, collisionFilterMask, - m_dispatcher1,0 - )) ; - - - - - + m_dispatcher1)); } - - -void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) +void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 minAabb, maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); //need to increase the aabb for contact thresholds - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { - btVector3 minAabb2,maxAabb2; - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + btVector3 minAabb2, maxAabb2; + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2); minAabb2 -= contactThreshold; maxAabb2 += contactThreshold; minAabb.setMin(minAabb2); @@ -166,10 +172,11 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + if (colObj->isStaticObject() || ((maxAabb - minAabb).length2() < btScalar(1e12))) { - bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } else + bp->setAabb(colObj->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1); + } + else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) @@ -187,14 +194,15 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) } } -void btCollisionWorld::updateAabbs() +void btCollisionWorld::updateAabbs() { BT_PROFILE("updateAabbs"); btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; + btAssert(colObj->getWorldArrayIndex() == i); //only update aabb of active objects if (m_forceUpdateAllAabbs || colObj->isActive()) @@ -204,14 +212,13 @@ void btCollisionWorld::updateAabbs() } } - -void btCollisionWorld::computeOverlappingPairs() +void btCollisionWorld::computeOverlappingPairs() { BT_PROFILE("calculateOverlappingPairs"); m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); } -void btCollisionWorld::performDiscreteCollisionDetection() +void btCollisionWorld::performDiscreteCollisionDetection() { BT_PROFILE("performDiscreteCollisionDetection"); @@ -225,53 +232,61 @@ void btCollisionWorld::performDiscreteCollisionDetection() { BT_PROFILE("dispatchAllCollisionPairs"); if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(), dispatchInfo, m_dispatcher1); } - } - - -void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) { - - //bool removeFromBroadphase = false; { - btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) { // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1); + getBroadphase()->destroyProxy(bp, m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } - - //swapremove - m_collisionObjects.remove(collisionObject); - + int iObj = collisionObject->getWorldArrayIndex(); + // btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously? + if (iObj >= 0 && iObj < m_collisionObjects.size()) + { + btAssert(collisionObject == m_collisionObjects[iObj]); + m_collisionObjects.swap(iObj, m_collisionObjects.size() - 1); + m_collisionObjects.pop_back(); + if (iObj < m_collisionObjects.size()) + { + m_collisionObjects[iObj]->setWorldArrayIndex(iObj); + } + } + else + { + // slow linear search + //swapremove + m_collisionObjects.remove(collisionObject); + } + collisionObject->setWorldArrayIndex(-1); } - -void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) +void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { - btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); - btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback); + btCollisionObjectWrapper colObWrap(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1); + btCollisionWorld::rayTestSingleInternal(rayFromTrans, rayToTrans, &colObWrap, resultCallback); } -void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, - const btCollisionObjectWrapper* collisionObjectWrap, - RayResultCallback& resultCallback) +void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + RayResultCallback& resultCallback) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); @@ -285,12 +300,12 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver); - - btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver); - + btConvexShape* convexShape = (btConvexShape*)collisionShape; + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast subSimplexConvexCaster(castShape, convexShape, &simplexSolver); + + btGjkConvexCast gjkConvexCaster(castShape, convexShape, &simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); btConvexCast* convexCasterPtr = 0; @@ -299,10 +314,10 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con convexCasterPtr = &gjkConvexCaster; else convexCasterPtr = &subSimplexConvexCaster; - + btConvexCast& convexCaster = *convexCasterPtr; - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -313,81 +328,105 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con #ifdef USE_SUBSIMPLEX_CONVEX_CAST //rotate normal into worldspace castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; -#endif //USE_SUBSIMPLEX_CONVEX_CAST +#endif //USE_SUBSIMPLEX_CONVEX_CAST castResult.m_normal.normalize(); - btCollisionWorld::LocalRayResult localRayResult - ( + btCollisionWorld::LocalRayResult localRayResult( collisionObjectWrap->getCollisionObject(), 0, castResult.m_normal, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localRayResult, normalInWorldSpace); - } } } - } else { + } + else + { if (collisionShape->isConcave()) { - //ConvexCast::CastResult - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + const btConcaveShape* m_triangleMesh; + + btTransform m_colObjWorldTransform; + + BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, const btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod + btTriangleRaycastCallback(from, to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) { - btCollisionWorld::RayResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - const btConcaveShape* m_triangleMesh; - - btTransform m_colObjWorldTransform; - - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape* triangleMesh,const btTransform& colObjWorldTransform): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) - { - } - - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; + } - btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + 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, - hitNormalWorld, - hitFraction); + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); - } + btCollisionWorld::LocalRayResult rayResult(m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); - }; + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); + } + }; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); // BT_PROFILE("rayTestConcave"); - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform); + + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; - triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + triangleMesh->performRaycast(&rcb, rayFromLocal, rayToLocal); + } + else if (collisionShape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btScaledBvhTriangleMeshShape + btScaledBvhTriangleMeshShape* scaledTriangleMesh = (btScaledBvhTriangleMeshShape*)collisionShape; + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)scaledTriangleMesh->getChildShape(); + + //scale the ray positions + btVector3 scale = scaledTriangleMesh->getLocalScaling(); + btVector3 rayFromLocalScaled = rayFromLocal / scale; + btVector3 rayToLocalScaled = rayToLocal / scale; + + //perform raycast in the underlying btBvhTriangleMeshShape + BridgeTriangleRaycastCallback rcb(rayFromLocalScaled, rayToLocalScaled, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled); + } + else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) + && collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE + ) + { + ///optimized version for btHeightfieldTerrainShape + btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + heightField->performRaycast(&rcb, rayFromLocal, rayToLocal); } else { @@ -404,45 +443,40 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback { btCollisionWorld::RayResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; btTransform m_colObjWorldTransform; - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) + BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod + btTriangleRaycastCallback(from, to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) { } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalWorld, - hitFraction); + btCollisionWorld::LocalRayResult rayResult(m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); } - }; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; @@ -450,9 +484,11 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); - concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&rcb, rayAabbMinLocal, rayAabbMaxLocal); } - } else { + } + else + { // BT_PROFILE("rayTestCompound"); if (collisionShape->isCompound()) { @@ -460,10 +496,10 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con { RayResultCallback* m_userCallback; int m_i; - - LocalInfoAdder2 (int i, RayResultCallback *user) + + LocalInfoAdder2(int i, RayResultCallback* user) : m_userCallback(user), m_i(i) - { + { m_closestHitFraction = m_userCallback->m_closestHitFraction; m_flags = m_userCallback->m_flags; } @@ -472,7 +508,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& r, bool b) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = -1; @@ -485,7 +521,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con return result; } }; - + struct RayTester : btDbvt::ICollide { const btCollisionObject* m_collisionObject; @@ -494,33 +530,29 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con const btTransform& m_rayFromTrans; const btTransform& m_rayToTrans; RayResultCallback& m_resultCallback; - + RayTester(const btCollisionObject* collisionObject, - const btCompoundShape* compoundShape, - const btTransform& colObjWorldTransform, - const btTransform& rayFromTrans, - const btTransform& rayToTrans, - RayResultCallback& resultCallback): - m_collisionObject(collisionObject), - m_compoundShape(compoundShape), - m_colObjWorldTransform(colObjWorldTransform), - m_rayFromTrans(rayFromTrans), - m_rayToTrans(rayToTrans), - m_resultCallback(resultCallback) + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + const btTransform& rayFromTrans, + const btTransform& rayToTrans, + RayResultCallback& resultCallback) : m_collisionObject(collisionObject), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_rayFromTrans(rayFromTrans), + m_rayToTrans(rayToTrans), + m_resultCallback(resultCallback) { - } - + void ProcessLeaf(int i) { const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); const btTransform& childTrans = m_compoundShape->getChildTransform(i); btTransform childWorldTrans = m_colObjWorldTransform * childTrans; - - btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i); - // replace collision shape so that callback can determine the triangle - + btCollisionObjectWrapper tmpOb(0, childCollisionShape, m_collisionObject, childWorldTrans, -1, i); + // replace collision shape so that callback can determine the triangle LocalInfoAdder2 my_cb(i, &m_resultCallback); @@ -529,19 +561,17 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con m_rayToTrans, &tmpOb, my_cb); - } - + void Process(const btDbvtNode* leaf) { ProcessLeaf(leaf->dataAsInt); } }; - + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); - RayTester rayCB( collisionObjectWrap->getCollisionObject(), compoundShape, @@ -549,39 +579,39 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con rayFromTrans, rayToTrans, resultCallback); -#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION +#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION if (dbvt) { btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); - btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); + btDbvt::rayTest(dbvt->m_root, localRayFrom, localRayTo, rayCB); } else -#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION +#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION { for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) { rayCB.ProcessLeaf(i); - } + } } } } } } -void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { - btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); - btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration); + btCollisionObjectWrapper tmpOb(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1); + btCollisionWorld::objectQuerySingleInternal(castShape, convexFromTrans, convexToTrans, &tmpOb, resultCallback, allowedPenetration); } -void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - const btCollisionObjectWrapper* colObjWrap, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) +void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { const btCollisionShape* collisionShape = colObjWrap->getCollisionShape(); const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform(); @@ -591,21 +621,19 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, //BT_PROFILE("convexSweepConvex"); btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; - castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? + castResult.m_fraction = resultCallback.m_closestHitFraction; //btScalar(1.);//?? - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + btConvexShape* convexShape = (btConvexShape*)collisionShape; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; - btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&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)) + if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -613,25 +641,24 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, if (castResult.m_fraction < resultCallback.m_closestHitFraction) { castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( + btCollisionWorld::LocalConvexResult localConvexResult( colObjWrap->getCollisionObject(), 0, castResult.m_normal, castResult.m_hitPoint, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); - } } } - } else { + } + else + { if (collisionShape->isConcave()) { - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { //BT_PROFILE("convexSweepbtBvhTriangleMesh"); btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; @@ -645,62 +672,57 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; - - BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + const btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const 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 ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { + btCollisionWorld::LocalConvexResult convexResult(m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - - bool normalInWorldSpace = true; + bool normalInWorldSpace = true; - - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace); } return hitFraction; } - }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); - triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); - } else + triangleMesh->performConvexcast(&tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal); + } + else { - if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE) + if (collisionShape->getShapeType() == STATIC_PLANE_PROXYTYPE) { btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction; - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape; - btContinuousConvexCollision convexCaster1(castShape,planeShape); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)collisionShape; + btContinuousConvexCollision convexCaster1(castShape, planeShape); btConvexCast* castPtr = &convexCaster1; - if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -708,22 +730,20 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, if (castResult.m_fraction < resultCallback.m_closestHitFraction) { castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( + btCollisionWorld::LocalConvexResult localConvexResult( colObjWrap->getCollisionObject(), 0, castResult.m_normal, castResult.m_hitPoint, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); } } } - - } else + } + else { //BT_PROFILE("convexSweepConcave"); btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; @@ -737,44 +757,39 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; - - BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* 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 ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { + btCollisionWorld::LocalConvexResult convexResult(m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - - bool normalInWorldSpace = true; + bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace); } return hitFraction; } - }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; @@ -786,27 +801,56 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, rayAabbMaxLocal.setMax(convexToLocal); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; - concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&tccb, rayAabbMinLocal, rayAabbMaxLocal); } } - } else { - ///@todo : use AABB tree or other BVH acceleration structure! + } + else + { if (collisionShape->isCompound()) { - BT_PROFILE("convexSweepCompound"); - const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); - int i=0; - for (i=0;i<compoundShape->getNumChildShapes();i++) + struct btCompoundLeafCallback : btDbvt::ICollide { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - - struct LocalInfoAdder : public ConvexResultCallback { - ConvexResultCallback* m_userCallback; + btCompoundLeafCallback( + const btCollisionObjectWrapper* colObjWrap, + const btConvexShape* castShape, + const btTransform& convexFromTrans, + const btTransform& convexToTrans, + btScalar allowedPenetration, + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback) + : m_colObjWrap(colObjWrap), + m_castShape(castShape), + m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_allowedPenetration(allowedPenetration), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_resultCallback(resultCallback) + { + } + + const btCollisionObjectWrapper* m_colObjWrap; + const btConvexShape* m_castShape; + const btTransform& m_convexFromTrans; + const btTransform& m_convexToTrans; + btScalar m_allowedPenetration; + const btCompoundShape* m_compoundShape; + const btTransform& m_colObjWorldTransform; + ConvexResultCallback& m_resultCallback; + + public: + void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) + { + btTransform childWorldTrans = m_colObjWorldTransform * childTrans; + + struct LocalInfoAdder : public ConvexResultCallback + { + ConvexResultCallback* m_userCallback; int m_i; - LocalInfoAdder (int i, ConvexResultCallback *user) + LocalInfoAdder(int i, ConvexResultCallback* user) : m_userCallback(user), m_i(i) { m_closestHitFraction = m_userCallback->m_closestHitFraction; @@ -815,60 +859,99 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, { return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = -1; - shapeInfo.m_triangleIndex = m_i; - if (r.m_localShapeInfo == NULL) - r.m_localShapeInfo = &shapeInfo; - const btScalar result = m_userCallback->addSingleResult(r, b); - m_closestHitFraction = m_userCallback->m_closestHitFraction; - return result; - - } - }; - - LocalInfoAdder my_cb(i, &resultCallback); - - btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i); - - objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans, - &tmpObj,my_cb, allowedPenetration); - + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = -1; + shapeInfo.m_triangleIndex = m_i; + if (r.m_localShapeInfo == NULL) + r.m_localShapeInfo = &shapeInfo; + const btScalar result = m_userCallback->addSingleResult(r, b); + m_closestHitFraction = m_userCallback->m_closestHitFraction; + return result; + } + }; + + LocalInfoAdder my_cb(index, &m_resultCallback); + + btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index); + + objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration); + } + + void Process(const btDbvtNode* leaf) + { + // Processing leaf node + int index = leaf->dataAsInt; + + btTransform childTrans = m_compoundShape->getChildTransform(index); + const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index); + + ProcessChild(index, childTrans, childCollisionShape); + } + }; + + BT_PROFILE("convexSweepCompound"); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); + + btVector3 fromLocalAabbMin, fromLocalAabbMax; + btVector3 toLocalAabbMin, toLocalAabbMax; + + castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax); + castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax); + + fromLocalAabbMin.setMin(toLocalAabbMin); + fromLocalAabbMax.setMax(toLocalAabbMax); + + btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans, + allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); + + const btDbvt* tree = compoundShape->getDynamicAabbTree(); + if (tree) + { + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); + tree->collideTV(tree->m_root, bounds, callback); + } + else + { + int i; + for (i = 0; i < compoundShape->getNumChildShapes(); i++) + { + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childTrans = compoundShape->getChildTransform(i); + callback.ProcessChild(i, childTrans, childCollisionShape); + } } } } } } - struct btSingleRayCallback : public btBroadphaseRayCallback { - - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btCollisionWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btCollisionWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btCollisionWorld* world, btCollisionWorld::RayResultCallback& resultCallback) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) { m_rayFromTrans.setIdentity(); m_rayFromTrans.setOrigin(m_rayFromWorld); m_rayToTrans.setIdentity(); m_rayToTrans.setOrigin(m_rayToWorld); - btVector3 rayDir = (rayToWorld-rayFromWorld); + btVector3 rayDir = (rayToWorld - rayFromWorld); - rayDir.normalize (); + rayDir.normalize(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; @@ -877,22 +960,19 @@ struct btSingleRayCallback : public btBroadphaseRayCallback m_signs[1] = m_rayDirectionInverse[1] < 0.0; m_signs[2] = m_rayDirectionInverse[2] < 0.0; - m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); - + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); } - - - virtual bool process(const btBroadphaseProxy* proxy) + virtual bool process(const btBroadphaseProxy* proxy) { ///terminate further ray tests, once the closestHitFraction reached zero if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) return false; - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -910,57 +990,53 @@ struct btSingleRayCallback : public btBroadphaseRayCallback //culling already done by broadphase //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) { - m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); } } return true; } }; -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const +void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const { //BT_PROFILE("rayTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test - btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + btSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i=0;i<this->getNumCollisionObjects();i++) + for (int i = 0; i < this->getNumCollisionObjects(); i++) { rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - struct btSingleSweepCallback : public btBroadphaseRayCallback { - - btTransform m_convexFromTrans; - btTransform m_convexToTrans; - btVector3 m_hitNormal; - const btCollisionWorld* m_world; - btCollisionWorld::ConvexResultCallback& m_resultCallback; - btScalar m_allowedCcdPenetration; + btTransform m_convexFromTrans; + btTransform m_convexToTrans; + btVector3 m_hitNormal; + const btCollisionWorld* m_world; + btCollisionWorld::ConvexResultCallback& m_resultCallback; + btScalar m_allowedCcdPenetration; const btConvexShape* m_castShape; - - btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration) - :m_convexFromTrans(convexFromTrans), - m_convexToTrans(convexToTrans), - m_world(world), - m_resultCallback(resultCallback), - m_allowedCcdPenetration(allowedPenetration), - m_castShape(castShape) + btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, const btCollisionWorld* world, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedPenetration) + : m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_world(world), + m_resultCallback(resultCallback), + m_allowedCcdPenetration(allowedPenetration), + m_castShape(castShape) { - btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); + btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin()); btVector3 rayDir = unnormalizedRayDir.normalized(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; @@ -971,109 +1047,102 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback m_signs[2] = m_rayDirectionInverse[2] < 0.0; m_lambda_max = rayDir.dot(unnormalizedRayDir); - } - virtual bool process(const btBroadphaseProxy* proxy) + virtual bool process(const btBroadphaseProxy* proxy) { ///terminate further convex sweep tests, once the closestHitFraction reached zero if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) return false; - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback, - m_allowedCcdPenetration); + m_world->objectQuerySingle(m_castShape, m_convexFromTrans, m_convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback, + m_allowedCcdPenetration); } return true; } }; - - -void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { - BT_PROFILE("convexSweepTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical - - - btTransform convexFromTrans,convexToTrans; + btTransform convexFromTrans, convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel); + btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0f, linVel, angVel); btVector3 zeroLinVel; - zeroLinVel.setValue(0,0,0); + zeroLinVel.setValue(0, 0, 0); btTransform R; - R.setIdentity (); - R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax); + R.setIdentity(); + R.setRotation(convexFromTrans.getRotation()); + castShape->calculateTemporalAabb(R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax); } #ifndef USE_BRUTEFORCE_RAYBROADPHASE - btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); + btSingleSweepCallback convexCB(castShape, convexFromWorld, convexToWorld, this, resultCallback, allowedCcdPenetration); - m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax); + m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(), convexToTrans.getOrigin(), convexCB, castShapeAabbMin, castShapeAabbMax); #else /// 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++) + for (i = 0; i < m_collisionObjects.size(); i++) { - btCollisionObject* collisionObject= m_collisionObjects[i]; + btCollisionObject* collisionObject = m_collisionObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + 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 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)) + if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) { - objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + objectQuerySingle(castShape, convexFromTrans, convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } -#endif //USE_BRUTEFORCE_RAYBROADPHASE +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - - struct btBridgedManifoldResult : public btManifoldResult { + btCollisionWorld::ContactResultCallback& m_resultCallback; - btCollisionWorld::ContactResultCallback& m_resultCallback; - - btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback ) - :btManifoldResult(obj0Wrap,obj1Wrap), - m_resultCallback(resultCallback) + btBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, btCollisionWorld::ContactResultCallback& resultCallback) + : btManifoldResult(obj0Wrap, obj1Wrap), + m_resultCallback(resultCallback) { } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); btVector3 pointA = pointInWorld + normalOnBInWorld * depth; @@ -1081,78 +1150,74 @@ struct btBridgedManifoldResult : public btManifoldResult btVector3 localB; if (isSwapped) { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else + } + else { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } - - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + + btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; - - //BP mod, store contact triangles. + + //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; - newPt.m_index0 = m_index1; - newPt.m_index1 = m_index0; - } else + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } + else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; } //experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; - m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); - + const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; + m_resultCallback.addSingleResult(newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1); } - }; - - struct btSingleContactCallback : public btBroadphaseAabbCallback { - btCollisionObject* m_collisionObject; - btCollisionWorld* m_world; - btCollisionWorld::ContactResultCallback& m_resultCallback; - - - btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback) - :m_collisionObject(collisionObject), - m_world(world), - m_resultCallback(resultCallback) + btCollisionWorld* m_world; + btCollisionWorld::ContactResultCallback& m_resultCallback; + + btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world, btCollisionWorld::ContactResultCallback& resultCallback) + : m_collisionObject(collisionObject), + m_world(world), + m_resultCallback(resultCallback) { } - virtual bool process(const btBroadphaseProxy* proxy) + virtual bool process(const btBroadphaseProxy* proxy) { - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; if (collisionObject == m_collisionObject) return true; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { - btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1); - btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1); + btCollisionObjectWrapper ob0(0, m_collisionObject->getCollisionShape(), m_collisionObject, m_collisionObject->getWorldTransform(), -1, -1); + btCollisionObjectWrapper ob1(0, collisionObject->getCollisionShape(), collisionObject, collisionObject->getWorldTransform(), -1, -1); - btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1); + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0, &ob1, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { - btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); + btBridgedManifoldResult contactPointResult(&ob0, &ob1, m_resultCallback); //discrete collision detection query - - algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult); + + algorithm->processCollision(&ob0, &ob1, m_world->getDispatchInfo(), &contactPointResult); algorithm->~btCollisionAlgorithm(); m_world->getDispatcher()->freeCollisionAlgorithm(algorithm); @@ -1162,294 +1227,271 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback } }; - ///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback. ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) -void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback) +void btCollisionWorld::contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback) { - btVector3 aabbMin,aabbMax; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax); - btSingleContactCallback contactCB(colObj,this,resultCallback); - - m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB); -} + btVector3 aabbMin, aabbMax; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), aabbMin, aabbMax); + btSingleContactCallback contactCB(colObj, this, resultCallback); + m_broadphasePairCache->aabbTest(aabbMin, aabbMax, contactCB); +} ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. ///it reports one or more contact points (including the one with deepest penetration) -void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback) +void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback) { - btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1); - btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1); + btCollisionObjectWrapper obA(0, colObjA->getCollisionShape(), colObjA, colObjA->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obB(0, colObjB->getCollisionShape(), colObjB, colObjB->getWorldTransform(), -1, -1); - btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB); + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA, &obB, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { - btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); + btBridgedManifoldResult contactPointResult(&obA, &obB, resultCallback); + contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold; //discrete collision detection query - algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult); + algorithm->processCollision(&obA, &obB, getDispatchInfo(), &contactPointResult); algorithm->~btCollisionAlgorithm(); getDispatcher()->freeCollisionAlgorithm(algorithm); } - } - - - class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback { - btIDebugDraw* m_debugDrawer; - btVector3 m_color; - btTransform m_worldTrans; + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; public: + DebugDrawcallback(btIDebugDraw* debugDrawer, const btTransform& worldTrans, const btVector3& color) : m_debugDrawer(debugDrawer), + m_color(color), + m_worldTrans(worldTrans) + { + } - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : - m_debugDrawer(debugDrawer), - m_color(color), - m_worldTrans(worldTrans) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - processTriangle(triangle,partId,triangleIndex); - } - - virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - - btVector3 wv0,wv1,wv2; - wv0 = m_worldTrans*triangle[0]; - wv1 = m_worldTrans*triangle[1]; - wv2 = m_worldTrans*triangle[2]; - btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.); - - if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals ) - { - btVector3 normal = (wv1-wv0).cross(wv2-wv0); - normal.normalize(); - btVector3 normalColor(1,1,0); - m_debugDrawer->drawLine(center,center+normal,normalColor); - } - m_debugDrawer->drawLine(wv0,wv1,m_color); - m_debugDrawer->drawLine(wv1,wv2,m_color); - m_debugDrawer->drawLine(wv2,wv0,m_color); - } -}; + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + processTriangle(triangle, partId, triangleIndex); + } + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + (void)partId; + (void)triangleIndex; + btVector3 wv0, wv1, wv2; + wv0 = m_worldTrans * triangle[0]; + wv1 = m_worldTrans * triangle[1]; + wv2 = m_worldTrans * triangle[2]; + btVector3 center = (wv0 + wv1 + wv2) * btScalar(1. / 3.); + + if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normal = (wv1 - wv0).cross(wv2 - wv0); + normal.normalize(); + btVector3 normalColor(1, 1, 0); + m_debugDrawer->drawLine(center, center + normal, normalColor); + } + m_debugDrawer->drawLine(wv0, wv1, m_color); + m_debugDrawer->drawLine(wv1, wv2, m_color); + m_debugDrawer->drawLine(wv2, wv0, m_color); + } +}; void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) { // Draw a small simplex at the center of the object if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) { - getDebugDrawer()->drawTransform(worldTransform,1); + getDebugDrawer()->drawTransform(worldTransform, .1); } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); - for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + for (int i = compoundShape->getNumChildShapes() - 1; i >= 0; i--) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* colShape = compoundShape->getChildShape(i); - debugDrawObject(worldTransform*childTrans,colShape,color); + debugDrawObject(worldTransform * childTrans, colShape, color); } - - } else + } + else { + switch (shape->getShapeType()) + { + case BOX_SHAPE_PROXYTYPE: + { + const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); + btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); + getDebugDrawer()->drawBox(-halfExtents, halfExtents, worldTransform, color); + break; + } - switch (shape->getShapeType()) - { - - case BOX_SHAPE_PROXYTYPE: - { - const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); - btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); - getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); - break; - } - - case SPHERE_SHAPE_PROXYTYPE: - { - const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - - getDebugDrawer()->drawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - { - const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); - - btTransform childTransform; - childTransform.setIdentity(); - - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) - { - childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); - getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); - } - - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); - - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - - int upAxis = capsuleShape->getUpAxis(); - getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); - break; - } - case CONE_SHAPE_PROXYTYPE: - { - const btConeShape* coneShape = static_cast<const btConeShape*>(shape); - btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); - btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - - int upAxis= coneShape->getConeUpIndex(); - getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); - break; - - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; - getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); - break; - } - - case STATIC_PLANE_PROXYTYPE: - { - const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); - break; - - } - default: - { - - /// for polyhedral shapes - if (shape->isPolyhedral()) - { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - - int i; - if (polyshape->getConvexPolyhedron()) - { - const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); - for (i=0;i<poly->m_faces.size();i++) - { - btVector3 centroid(0,0,0); - int numVerts = poly->m_faces[i].m_indices.size(); - if (numVerts) - { - int lastV = poly->m_faces[i].m_indices[numVerts-1]; - for (int v=0;v<poly->m_faces[i].m_indices.size();v++) - { - int curVert = poly->m_faces[i].m_indices[v]; - centroid+=poly->m_vertices[curVert]; - getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); - lastV = curVert; - } - } - centroid*= btScalar(1.f)/btScalar(numVerts); - if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) - { - btVector3 normalColor(1,1,0); - btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); - getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); - } - - } - - - } else - { - for (i=0;i<polyshape->getNumEdges();i++) - { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); - } - } - - - } - - if (shape->isConcave()) - { - btConcaveShape* concaveMesh = (btConcaveShape*) shape; - - ///@todo pass camera, for some culling? no -> we are not a graphics lib - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); - - } - - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) - { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - } - - - - } - + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); + btScalar radius = sphereShape->getMargin(); //radius doesn't include the margin, so draw with margin + + getDebugDrawer()->drawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); + + btTransform childTransform; + childTransform.setIdentity(); + + for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--) + { + childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); + getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform * childTransform, color); + } + + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); + + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + + int upAxis = capsuleShape->getUpAxis(); + getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); + break; + } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + btScalar radius = coneShape->getRadius(); //+coneShape->getMargin(); + btScalar height = coneShape->getHeight(); //+coneShape->getMargin(); + + int upAxis = coneShape->getConeUpIndex(); + getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); + break; + } + case CYLINDER_SHAPE_PROXYTYPE: + { + const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); + break; + } + + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + getDebugDrawer()->drawPlane(planeNormal, planeConst, worldTransform, color); + break; + } + default: + { + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*)shape; + + int i; + if (polyshape->getConvexPolyhedron()) + { + const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); + for (i = 0; i < poly->m_faces.size(); i++) + { + btVector3 centroid(0, 0, 0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts) + { + int lastV = poly->m_faces[i].m_indices[numVerts - 1]; + for (int v = 0; v < poly->m_faces[i].m_indices.size(); v++) + { + int curVert = poly->m_faces[i].m_indices[v]; + centroid += poly->m_vertices[curVert]; + getDebugDrawer()->drawLine(worldTransform * poly->m_vertices[lastV], worldTransform * poly->m_vertices[curVert], color); + lastV = curVert; + } + } + centroid *= btScalar(1.f) / btScalar(numVerts); + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normalColor(1, 1, 0); + btVector3 faceNormal(poly->m_faces[i].m_plane[0], poly->m_faces[i].m_plane[1], poly->m_faces[i].m_plane[2]); + getDebugDrawer()->drawLine(worldTransform * centroid, worldTransform * (centroid + faceNormal), normalColor); + } + } + } + else + { + for (i = 0; i < polyshape->getNumEdges(); i++) + { + btVector3 a, b; + polyshape->getEdge(i, a, b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa, wb, color); + } + } + } + + if (shape->isConcave()) + { + btConcaveShape* concaveMesh = (btConcaveShape*)shape; + + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + + DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color); + concaveMesh->processAllTriangles(&drawCallback, aabbMin, aabbMax); + } + + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*)shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback, aabbMin, aabbMax); + } + } } } } - -void btCollisionWorld::debugDrawWorld() +void btCollisionWorld::debugDrawWorld() { if (getDebugDrawer()) { + getDebugDrawer()->clearLines(); + btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors(); - if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { - - if (getDispatcher()) { int numManifolds = getDispatcher()->getNumManifolds(); - - for (int i=0;i<numManifolds;i++) + + for (int i = 0; i < numManifolds; i++) { btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0()); //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1()); int numContacts = contactManifold->getNumContacts(); - for (int j=0;j<numContacts;j++) + for (int j = 0; j < numContacts; j++) { btManifoldPoint& cp = contactManifold->getContactPoint(j); - getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),defaultColors.m_contactPoint); + getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB, cp.m_normalWorldOnB, cp.getDistance(), cp.getLifeTime(), defaultColors.m_contactPoint); } } } @@ -1459,56 +1501,63 @@ void btCollisionWorld::debugDrawWorld() { int i; - for ( i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; - if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) + if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT) == 0) { if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { - btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4)); + btVector3 color(btScalar(0.4), btScalar(0.4), btScalar(0.4)); - switch(colObj->getActivationState()) + switch (colObj->getActivationState()) { - case ACTIVE_TAG: - color = defaultColors.m_activeObject; break; - case ISLAND_SLEEPING: - color = defaultColors.m_deactivatedObject;break; - case WANTS_DEACTIVATION: - color = defaultColors.m_wantsDeactivationObject;break; - case DISABLE_DEACTIVATION: - color = defaultColors.m_disabledDeactivationObject;break; - case DISABLE_SIMULATION: - color = defaultColors.m_disabledSimulationObject;break; - default: + case ACTIVE_TAG: + color = defaultColors.m_activeObject; + break; + case ISLAND_SLEEPING: + color = defaultColors.m_deactivatedObject; + break; + case WANTS_DEACTIVATION: + color = defaultColors.m_wantsDeactivationObject; + break; + case DISABLE_DEACTIVATION: + color = defaultColors.m_disabledDeactivationObject; + break; + case DISABLE_SIMULATION: + color = defaultColors.m_disabledSimulationObject; + break; + default: { - color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3)); + color = btVector3(btScalar(.3), btScalar(0.3), btScalar(0.3)); } }; - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + colObj->getCustomDebugColor(color); + + debugDrawObject(colObj->getWorldTransform(), colObj->getCollisionShape(), color); } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - btVector3 minAabb,maxAabb; + btVector3 minAabb, maxAabb; btVector3 colorvec = defaultColors.m_aabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); + btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; - btVector3 minAabb2,maxAabb2; + btVector3 minAabb2, maxAabb2; - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2); minAabb2 -= contactThreshold; maxAabb2 += contactThreshold; minAabb.setMin(minAabb2); maxAabb.setMax(maxAabb2); } - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + m_debugDrawer->drawAabb(minAabb, maxAabb, colorvec); } } } @@ -1516,45 +1565,63 @@ void btCollisionWorld::debugDrawWorld() } } - -void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) +void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) { int i; ///keep track of shapes already serialized - btHashMap<btHashPtr,btCollisionShape*> serializedShapes; + btHashMap<btHashPtr, btCollisionShape*> serializedShapes; - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btCollisionShape* shape = colObj->getCollisionShape(); if (!serializedShapes.find(shape)) { - serializedShapes.insert(shape,shape); + serializedShapes.insert(shape, shape); shape->serializeSingleShape(serializer); } } //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; - if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)) + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) { colObj->serializeSingleObject(serializer); } } } - -void btCollisionWorld::serialize(btSerializer* serializer) +void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) { + if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS) + { + int numManifolds = getDispatcher()->getNumManifolds(); + for (int i = 0; i < numManifolds; i++) + { + const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i]; + //don't serialize empty manifolds, they just take space + //(may have to do it anyway if it destroys determinism) + if (manifold->getNumContacts() == 0) + continue; + + btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1); + const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold); + } + } +} +void btCollisionWorld::serialize(btSerializer* serializer) +{ serializer->startSerialization(); - + serializeCollisionObjects(serializer); - + + serializeContactManifolds(serializer); + serializer->finishSerialization(); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index ec40c969f51..fd0e5b9bbdd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -13,9 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - /** - * @page Bullet Documentation + * @mainpage Bullet Documentation * * @section intro_sec Introduction * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). @@ -66,8 +65,6 @@ subject to the following restrictions: * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf * */ - - #ifndef BT_COLLISION_WORLD_H #define BT_COLLISION_WORLD_H @@ -87,144 +84,138 @@ class btSerializer; ///CollisionWorld is interface and container for the collision detection class btCollisionWorld { - - protected: + btAlignedObjectArray<btCollisionObject*> m_collisionObjects; - btAlignedObjectArray<btCollisionObject*> m_collisionObjects; - - btDispatcher* m_dispatcher1; + btDispatcher* m_dispatcher1; - btDispatcherInfo m_dispatchInfo; + btDispatcherInfo m_dispatchInfo; - btBroadphaseInterface* m_broadphasePairCache; + btBroadphaseInterface* m_broadphasePairCache; - btIDebugDraw* m_debugDrawer; + btIDebugDraw* m_debugDrawer; ///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs ///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB) bool m_forceUpdateAllAabbs; - void serializeCollisionObjects(btSerializer* serializer); + void serializeCollisionObjects(btSerializer* serializer); -public: + void serializeContactManifolds(btSerializer* serializer); +public: //this constructor doesn't own the dispatcher and paircache/broadphase - btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); + btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionWorld(); - void setBroadphase(btBroadphaseInterface* pairCache) + void setBroadphase(btBroadphaseInterface* pairCache) { m_broadphasePairCache = pairCache; } - const btBroadphaseInterface* getBroadphase() const + const btBroadphaseInterface* getBroadphase() const { return m_broadphasePairCache; } - btBroadphaseInterface* getBroadphase() + btBroadphaseInterface* getBroadphase() { return m_broadphasePairCache; } - btOverlappingPairCache* getPairCache() + btOverlappingPairCache* getPairCache() { return m_broadphasePairCache->getOverlappingPairCache(); } - - btDispatcher* getDispatcher() + btDispatcher* getDispatcher() { return m_dispatcher1; } - const btDispatcher* getDispatcher() const + const btDispatcher* getDispatcher() const { return m_dispatcher1; } - void updateSingleAabb(btCollisionObject* colObj); + void updateSingleAabb(btCollisionObject* colObj); - virtual void updateAabbs(); + virtual void updateAabbs(); ///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation) ///it can be useful to use if you perform ray tests without collision detection/simulation - virtual void computeOverlappingPairs(); + virtual void computeOverlappingPairs(); - - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) { - m_debugDrawer = debugDrawer; + m_debugDrawer = debugDrawer; } - virtual btIDebugDraw* getDebugDrawer() + virtual btIDebugDraw* getDebugDrawer() { return m_debugDrawer; } - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); - ///LocalShapeInfo gives extra information for complex shapes ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart - struct LocalShapeInfo + struct LocalShapeInfo { - int m_shapePart; - int m_triangleIndex; - + int m_shapePart; + int m_triangleIndex; + //const btCollisionShape* m_shapeTemp; //const btTransform* m_shapeLocalTransform; }; - struct LocalRayResult + struct LocalRayResult { - LocalRayResult(const btCollisionObject* collisionObject, - LocalShapeInfo* localShapeInfo, - const btVector3& hitNormalLocal, - btScalar hitFraction) - :m_collisionObject(collisionObject), - m_localShapeInfo(localShapeInfo), - m_hitNormalLocal(hitNormalLocal), - m_hitFraction(hitFraction) + LocalRayResult(const btCollisionObject* collisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + btScalar hitFraction) + : m_collisionObject(collisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitFraction(hitFraction) { } - const btCollisionObject* m_collisionObject; - LocalShapeInfo* m_localShapeInfo; - btVector3 m_hitNormalLocal; - btScalar m_hitFraction; - + const btCollisionObject* m_collisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btScalar m_hitFraction; }; ///RayResultCallback is used to report new raycast results - struct RayResultCallback + struct RayResultCallback { - btScalar m_closestHitFraction; - const btCollisionObject* m_collisionObject; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; + btScalar m_closestHitFraction; + const btCollisionObject* m_collisionObject; + int m_collisionFilterGroup; + int m_collisionFilterMask; //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke. unsigned int m_flags; virtual ~RayResultCallback() { } - bool hasHit() const + bool hasHit() const { return (m_collisionObject != 0); } RayResultCallback() - :m_closestHitFraction(btScalar(1.)), - m_collisionObject(0), - m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter), - //@BP Mod - m_flags(0) + : m_closestHitFraction(btScalar(1.)), + m_collisionObject(0), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + //@BP Mod + m_flags(0) { } @@ -235,62 +226,62 @@ public: return collides; } - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0; + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) = 0; }; - struct ClosestRayResultCallback : public RayResultCallback + struct ClosestRayResultCallback : public RayResultCallback { - ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld) + ClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) { } - btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_rayToWorld; + btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; - btVector3 m_hitNormalWorld; - btVector3 m_hitPointWorld; - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) { //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; if (normalInWorldSpace) { m_hitNormalWorld = rayResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; } - m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); return rayResult.m_hitFraction; } }; - struct AllHitsRayResultCallback : public RayResultCallback + struct AllHitsRayResultCallback : public RayResultCallback { - AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld) + AllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) { } - btAlignedObjectArray<const btCollisionObject*> m_collisionObjects; + btAlignedObjectArray<const btCollisionObject*> m_collisionObjects; - btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_rayToWorld; + btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; - btAlignedObjectArray<btVector3> m_hitNormalWorld; - btAlignedObjectArray<btVector3> m_hitPointWorld; + btAlignedObjectArray<btVector3> m_hitNormalWorld; + btAlignedObjectArray<btVector3> m_hitPointWorld; btAlignedObjectArray<btScalar> m_hitFractions; - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) { m_collisionObject = rayResult.m_collisionObject; m_collisionObjects.push_back(rayResult.m_collisionObject); @@ -298,69 +289,66 @@ public: if (normalInWorldSpace) { hitNormalWorld = rayResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; } m_hitNormalWorld.push_back(hitNormalWorld); btVector3 hitPointWorld; - hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); m_hitPointWorld.push_back(hitPointWorld); m_hitFractions.push_back(rayResult.m_hitFraction); return m_closestHitFraction; } }; - struct LocalConvexResult { - LocalConvexResult(const 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) + LocalConvexResult(const 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) { } - const btCollisionObject* m_hitCollisionObject; - LocalShapeInfo* m_localShapeInfo; - btVector3 m_hitNormalLocal; - btVector3 m_hitPointLocal; - btScalar m_hitFraction; + const 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 + struct ConvexResultCallback { - btScalar m_closestHitFraction; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - + btScalar m_closestHitFraction; + int m_collisionFilterGroup; + int m_collisionFilterMask; + ConvexResultCallback() - :m_closestHitFraction(btScalar(1.)), - m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + : m_closestHitFraction(btScalar(1.)), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) { } virtual ~ConvexResultCallback() { } - - bool hasHit() const + + bool hasHit() const { return (m_closestHitFraction < btScalar(1.)); } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const { bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; @@ -368,39 +356,40 @@ public: return collides; } - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; + virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) = 0; }; - struct ClosestConvexResultCallback : public ConvexResultCallback + struct ClosestConvexResultCallback : public ConvexResultCallback { - ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld) - :m_convexFromWorld(convexFromWorld), - m_convexToWorld(convexToWorld), - m_hitCollisionObject(0) + 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_convexFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_convexToWorld; - btVector3 m_hitNormalWorld; - btVector3 m_hitPointWorld; - const btCollisionObject* m_hitCollisionObject; - - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + const btCollisionObject* m_hitCollisionObject; + + virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) { -//caller already does the filter on the m_closestHitFraction + //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 + } + else { ///need to transform normal into worldspace - m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal; } m_hitPointWorld = convexResult.m_hitPointLocal; return convexResult.m_hitFraction; @@ -408,21 +397,23 @@ public: }; ///ContactResultCallback is used to report contact points - struct ContactResultCallback + struct ContactResultCallback { - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - + int m_collisionFilterGroup; + int m_collisionFilterMask; + btScalar m_closestDistanceThreshold; + ContactResultCallback() - :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + : m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + m_closestDistanceThreshold(0) { } virtual ~ContactResultCallback() { } - + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; @@ -430,58 +421,57 @@ public: return collides; } - virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0; + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) = 0; }; - - - int getNumCollisionObjects() const + int getNumCollisionObjects() const { return int(m_collisionObjects.size()); } /// 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. - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + virtual 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, btScalar allowedCcdPenetration = btScalar(0.)) const; + void convexSweepTest(const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; ///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback. ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) - void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); + void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. ///it reports one or more contact points (including the one with deepest penetration) - void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback); - + void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback); /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. /// This allows more customization. - static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback); + static void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); - static void rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, - const btCollisionObjectWrapper* collisionObjectWrap, - RayResultCallback& resultCallback); + static void rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + 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, - ConvexResultCallback& resultCallback, btScalar allowedPenetration); + static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); + + static void objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); - static void objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - const btCollisionObjectWrapper* colObjWrap, - ConvexResultCallback& resultCallback, btScalar allowedPenetration); + virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject); btCollisionObjectArray& getCollisionObjectArray() { @@ -493,10 +483,9 @@ public: return m_collisionObjects; } + virtual void removeCollisionObject(btCollisionObject* collisionObject); - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - virtual void performDiscreteCollisionDetection(); + virtual void performDiscreteCollisionDetection(); btDispatcherInfo& getDispatchInfo() { @@ -507,20 +496,18 @@ public: { return m_dispatchInfo; } - - bool getForceUpdateAllAabbs() const + + bool getForceUpdateAllAabbs() const { return m_forceUpdateAllAabbs; } - void setForceUpdateAllAabbs( bool forceUpdateAllAabbs) + void setForceUpdateAllAabbs(bool forceUpdateAllAabbs) { m_forceUpdateAllAabbs = forceUpdateAllAabbs; } ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo) - virtual void serialize(btSerializer* serializer); - + virtual void serialize(btSerializer* serializer); }; - -#endif //BT_COLLISION_WORLD_H +#endif //BT_COLLISION_WORLD_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp index 57eb81703e7..e56e73dcf5b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp @@ -15,269 +15,251 @@ subject to the following restrictions: #include "btCollisionWorldImporter.h" #include "btBulletCollisionCommon.h" -#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition +#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT #include "BulletCollision/Gimpact/btGImpactShape.h" -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world) -:m_collisionWorld(world), -m_verboseMode(0) + : m_collisionWorld(world), + m_verboseMode(0) { - } btCollisionWorldImporter::~btCollisionWorldImporter() { } - - - - -bool btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays) +bool btCollisionWorldImporter::convertAllObjects(btBulletSerializedArrays* arrays) { - m_shapeMap.clear(); m_bodyMap.clear(); int i; - for (i=0;i<arrays->m_bvhsDouble.size();i++) + for (i = 0; i < arrays->m_bvhsDouble.size(); i++) { btOptimizedBvh* bvh = createOptimizedBvh(); btQuantizedBvhDoubleData* bvhData = arrays->m_bvhsDouble[i]; bvh->deSerializeDouble(*bvhData); - m_bvhMap.insert(arrays->m_bvhsDouble[i],bvh); + m_bvhMap.insert(arrays->m_bvhsDouble[i], bvh); } - for (i=0;i<arrays->m_bvhsFloat.size();i++) - { - btOptimizedBvh* bvh = createOptimizedBvh(); - btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; + for (i = 0; i < arrays->m_bvhsFloat.size(); i++) + { + btOptimizedBvh* bvh = createOptimizedBvh(); + btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; bvh->deSerializeFloat(*bvhData); - m_bvhMap.insert(arrays->m_bvhsFloat[i],bvh); + m_bvhMap.insert(arrays->m_bvhsFloat[i], bvh); } - - - - - for (i=0;i<arrays->m_colShapeData.size();i++) + for (i = 0; i < arrays->m_colShapeData.size(); i++) { btCollisionShapeData* shapeData = arrays->m_colShapeData[i]; btCollisionShape* shape = convertCollisionShape(shapeData); if (shape) { - // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); - m_shapeMap.insert(shapeData,shape); + // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); + m_shapeMap.insert(shapeData, shape); } - if (shape&& shapeData->m_name) + if (shape && shapeData->m_name) { char* newname = duplicateName(shapeData->m_name); - m_objectNameMap.insert(shape,newname); - m_nameShapeMap.insert(newname,shape); + m_objectNameMap.insert(shape, newname); + m_nameShapeMap.insert(newname, shape); } } - - for (i=0;i<arrays->m_collisionObjectDataDouble.size();i++) + for (i = 0; i < arrays->m_collisionObjectDataDouble.size(); i++) { - btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; - btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); - if (shapePtr && *shapePtr) - { - btTransform startTransform; - colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; - startTransform.deSerializeDouble(colObjData->m_worldTransform); - - btCollisionShape* shape = (btCollisionShape*)*shapePtr; - btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); - body->setFriction(btScalar(colObjData->m_friction)); - body->setRestitution(btScalar(colObjData->m_restitution)); + btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeDouble(colObjData->m_worldTransform); + + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name); + body->setFriction(btScalar(colObjData->m_friction)); + body->setRestitution(btScalar(colObjData->m_restitution)); #ifdef USE_INTERNAL_EDGE_UTILITY - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; - if (trimesh->getTriangleInfoMap()) - { - body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); - } - } -#endif //USE_INTERNAL_EDGE_UTILITY - m_bodyMap.insert(colObjData,body); - } else - { - printf("error: no shape found\n"); - } + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData, body); + } + else + { + printf("error: no shape found\n"); + } } - for (i=0;i<arrays->m_collisionObjectDataFloat.size();i++) + for (i = 0; i < arrays->m_collisionObjectDataFloat.size(); i++) { - btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; - btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); - if (shapePtr && *shapePtr) - { - btTransform startTransform; - colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; - startTransform.deSerializeFloat(colObjData->m_worldTransform); + btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeFloat(colObjData->m_worldTransform); - btCollisionShape* shape = (btCollisionShape*)*shapePtr; - btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; - if (trimesh->getTriangleInfoMap()) - { - body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); - } - } -#endif //USE_INTERNAL_EDGE_UTILITY - m_bodyMap.insert(colObjData,body); - } else - { - printf("error: no shape found\n"); - } - } + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData, body); + } + else + { + printf("error: no shape found\n"); + } + } return true; } - - void btCollisionWorldImporter::deleteAllData() { int i; - for (i=0;i<m_allocatedCollisionObjects.size();i++) + for (i = 0; i < m_allocatedCollisionObjects.size(); i++) { - if(m_collisionWorld) + if (m_collisionWorld) m_collisionWorld->removeCollisionObject(m_allocatedCollisionObjects[i]); delete m_allocatedCollisionObjects[i]; } m_allocatedCollisionObjects.clear(); - - for (i=0;i<m_allocatedCollisionShapes.size();i++) + for (i = 0; i < m_allocatedCollisionShapes.size(); i++) { delete m_allocatedCollisionShapes[i]; } m_allocatedCollisionShapes.clear(); - - for (i=0;i<m_allocatedBvhs.size();i++) + for (i = 0; i < m_allocatedBvhs.size(); i++) { delete m_allocatedBvhs[i]; } m_allocatedBvhs.clear(); - for (i=0;i<m_allocatedTriangleInfoMaps.size();i++) + for (i = 0; i < m_allocatedTriangleInfoMaps.size(); i++) { delete m_allocatedTriangleInfoMaps[i]; } m_allocatedTriangleInfoMaps.clear(); - for (i=0;i<m_allocatedTriangleIndexArrays.size();i++) + for (i = 0; i < m_allocatedTriangleIndexArrays.size(); i++) { delete m_allocatedTriangleIndexArrays[i]; } m_allocatedTriangleIndexArrays.clear(); - for (i=0;i<m_allocatedNames.size();i++) + for (i = 0; i < m_allocatedNames.size(); i++) { delete[] m_allocatedNames[i]; } m_allocatedNames.clear(); - for (i=0;i<m_allocatedbtStridingMeshInterfaceDatas.size();i++) + for (i = 0; i < m_allocatedbtStridingMeshInterfaceDatas.size(); i++) { btStridingMeshInterfaceData* curData = m_allocatedbtStridingMeshInterfaceDatas[i]; - for(int a = 0;a < curData->m_numMeshParts;a++) + for (int a = 0; a < curData->m_numMeshParts; a++) { btMeshPartData* curPart = &curData->m_meshPartsPtr[a]; - if(curPart->m_vertices3f) - delete [] curPart->m_vertices3f; + if (curPart->m_vertices3f) + delete[] curPart->m_vertices3f; - if(curPart->m_vertices3d) - delete [] curPart->m_vertices3d; + if (curPart->m_vertices3d) + delete[] curPart->m_vertices3d; - if(curPart->m_indices32) - delete [] curPart->m_indices32; + if (curPart->m_indices32) + delete[] curPart->m_indices32; - if(curPart->m_3indices16) - delete [] curPart->m_3indices16; + if (curPart->m_3indices16) + delete[] curPart->m_3indices16; - if(curPart->m_indices16) - delete [] curPart->m_indices16; + if (curPart->m_indices16) + delete[] curPart->m_indices16; if (curPart->m_3indices8) - delete [] curPart->m_3indices8; - + delete[] curPart->m_3indices8; } - delete [] curData->m_meshPartsPtr; + delete[] curData->m_meshPartsPtr; delete curData; } m_allocatedbtStridingMeshInterfaceDatas.clear(); - for (i=0;i<m_indexArrays.size();i++) + for (i = 0; i < m_indexArrays.size(); i++) { btAlignedFree(m_indexArrays[i]); } - m_indexArrays.clear(); + m_indexArrays.clear(); - for (i=0;i<m_shortIndexArrays.size();i++) + for (i = 0; i < m_shortIndexArrays.size(); i++) { btAlignedFree(m_shortIndexArrays[i]); } - m_shortIndexArrays.clear(); + m_shortIndexArrays.clear(); - for (i=0;i<m_charIndexArrays.size();i++) + for (i = 0; i < m_charIndexArrays.size(); i++) { btAlignedFree(m_charIndexArrays[i]); } - m_charIndexArrays.clear(); + m_charIndexArrays.clear(); - for (i=0;i<m_floatVertexArrays.size();i++) + for (i = 0; i < m_floatVertexArrays.size(); i++) { btAlignedFree(m_floatVertexArrays[i]); } - m_floatVertexArrays.clear(); + m_floatVertexArrays.clear(); - for (i=0;i<m_doubleVertexArrays.size();i++) + for (i = 0; i < m_doubleVertexArrays.size(); i++) { btAlignedFree(m_doubleVertexArrays[i]); } - m_doubleVertexArrays.clear(); - - + m_doubleVertexArrays.clear(); } - - -btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionShapeData* shapeData ) +btCollisionShape* btCollisionWorldImporter::convertCollisionShape(btCollisionShapeData* shapeData) { btCollisionShape* shape = 0; switch (shapeData->m_shapeType) - { - case STATIC_PLANE_PROXYTYPE: + { + case STATIC_PLANE_PROXYTYPE: { btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData; - btVector3 planeNormal,localScaling; + btVector3 planeNormal, localScaling; planeNormal.deSerializeFloat(planeData->m_planeNormal); localScaling.deSerializeFloat(planeData->m_localScaling); - shape = createPlaneShape(planeNormal,planeData->m_planeConstant); + shape = createPlaneShape(planeNormal, planeData->m_planeConstant); shape->setLocalScaling(localScaling); break; } - case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: + case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: { - btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData; - btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData; + btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*)shapeData; + btCollisionShapeData* colShapeData = (btCollisionShapeData*)&scaledMesh->m_trimeshShapeData; colShapeData->m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; btCollisionShape* childShape = convertCollisionShape(colShapeData); btBvhTriangleMeshShape* meshShape = (btBvhTriangleMeshShape*)childShape; @@ -288,15 +270,14 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS break; } #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT - case GIMPACT_SHAPE_PROXYTYPE: + case GIMPACT_SHAPE_PROXYTYPE: { - btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData; + btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*)shapeData; if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE) { btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&gimpactData->m_meshInterface); btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData); - btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface); btVector3 localScaling; localScaling.deSerializeFloat(gimpactData->m_localScaling); @@ -304,47 +285,45 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin)); gimpactShape->updateBound(); shape = gimpactShape; - } else + } + else { printf("unsupported gimpact sub type\n"); } break; } -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT - //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API - //so deal with this +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API + //so deal with this case CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; - switch (capData->m_upAxis) { - case 0: + case 0: { - shape = createCapsuleShapeX(1,1); + shape = createCapsuleShapeX(1, 1); break; } - case 1: + case 1: { - shape = createCapsuleShapeY(1,1); + shape = createCapsuleShapeY(1, 1); break; } - case 2: + case 2: { - shape = createCapsuleShapeZ(1,1); + shape = createCapsuleShapeZ(1, 1); break; } - default: + default: { printf("error: wrong up axis for btCapsuleShape\n"); } - - }; if (shape) { - btCapsuleShape* cap = (btCapsuleShape*) shape; + btCapsuleShape* cap = (btCapsuleShape*)shape; cap->deSerializeFloat(capData); } break; @@ -355,163 +334,156 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS case SPHERE_SHAPE_PROXYTYPE: case MULTI_SPHERE_SHAPE_PROXYTYPE: case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; + btVector3 implicitShapeDimensions; + implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + btVector3 margin(bsd->m_collisionMargin, bsd->m_collisionMargin, bsd->m_collisionMargin); + switch (shapeData->m_shapeType) { - btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; - btVector3 implicitShapeDimensions; - implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); - btVector3 localScaling; - localScaling.deSerializeFloat(bsd->m_localScaling); - btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin); - switch (shapeData->m_shapeType) + case BOX_SHAPE_PROXYTYPE: { - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin); - //box->initializePolyhedralFeatures(); - shape = box; + btBoxShape* box = (btBoxShape*)createBoxShape(implicitShapeDimensions / localScaling + margin); + //box->initializePolyhedralFeatures(); + shape = box; + break; + } + case SPHERE_SHAPE_PROXYTYPE: + { + shape = createSphereShape(implicitShapeDimensions.getX()); + break; + } + + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShapeData* cylData = (btCylinderShapeData*)shapeData; + btVector3 halfExtents = implicitShapeDimensions + margin; + switch (cylData->m_upAxis) + { + case 0: + { + shape = createCylinderShapeX(halfExtents.getY(), halfExtents.getX()); break; } - case SPHERE_SHAPE_PROXYTYPE: + case 1: { - shape = createSphereShape(implicitShapeDimensions.getX()); + shape = createCylinderShapeY(halfExtents.getX(), halfExtents.getY()); break; } - - case CYLINDER_SHAPE_PROXYTYPE: + case 2: { - btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData; - btVector3 halfExtents = implicitShapeDimensions+margin; - switch (cylData->m_upAxis) - { - case 0: - { - shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX()); - break; - } - case 1: - { - shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY()); - break; - } - case 2: - { - shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ()); - break; - } - default: - { - printf("unknown Cylinder up axis\n"); - } - - }; - - - + shape = createCylinderShapeZ(halfExtents.getX(), halfExtents.getZ()); break; } - case CONE_SHAPE_PROXYTYPE: + default: { - btConeShapeData* conData = (btConeShapeData*) shapeData; - btVector3 halfExtents = implicitShapeDimensions;//+margin; - switch (conData->m_upIndex) - { - case 0: - { - shape = createConeShapeX(halfExtents.getY(),halfExtents.getX()); - break; - } - case 1: - { - shape = createConeShapeY(halfExtents.getX(),halfExtents.getY()); - break; - } - case 2: - { - shape = createConeShapeZ(halfExtents.getX(),halfExtents.getZ()); - break; - } - default: - { - printf("unknown Cone up axis\n"); - } - - }; - - + printf("unknown Cylinder up axis\n"); + } + }; + break; + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShapeData* conData = (btConeShapeData*)shapeData; + btVector3 halfExtents = implicitShapeDimensions; //+margin; + switch (conData->m_upIndex) + { + case 0: + { + shape = createConeShapeX(halfExtents.getY(), halfExtents.getX()); break; } - case MULTI_SPHERE_SHAPE_PROXYTYPE: + case 1: { - btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; - int numSpheres = mss->m_localPositionArraySize; - - btAlignedObjectArray<btVector3> tmpPos; - btAlignedObjectArray<btScalar> radii; - radii.resize(numSpheres); - tmpPos.resize(numSpheres); - int i; - for ( i=0;i<numSpheres;i++) - { - tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); - radii[i] = mss->m_localPositionArrayPtr[i].m_radius; - } - shape = createMultiSphereShape(&tmpPos[0],&radii[0],numSpheres); + shape = createConeShapeY(halfExtents.getX(), halfExtents.getY()); break; } - case CONVEX_HULL_SHAPE_PROXYTYPE: + case 2: { - // int sz = sizeof(btConvexHullShapeData); - // int sz2 = sizeof(btConvexInternalShapeData); - // int sz3 = sizeof(btCollisionShapeData); - btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; - int numPoints = convexData->m_numUnscaledPoints; - - btAlignedObjectArray<btVector3> tmpPoints; - tmpPoints.resize(numPoints); - int i; - for ( i=0;i<numPoints;i++) - { -#ifdef BT_USE_DOUBLE_PRECISION - if (convexData->m_unscaledPointsDoublePtr) - tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); - if (convexData->m_unscaledPointsFloatPtr) - tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); -#else - if (convexData->m_unscaledPointsFloatPtr) - tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); - if (convexData->m_unscaledPointsDoublePtr) - tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); -#endif //BT_USE_DOUBLE_PRECISION - } - btConvexHullShape* hullShape = createConvexHullShape(); - for (i=0;i<numPoints;i++) - { - hullShape->addPoint(tmpPoints[i]); - } - hullShape->setMargin(bsd->m_collisionMargin); - //hullShape->initializePolyhedralFeatures(); - shape = hullShape; + shape = createConeShapeZ(halfExtents.getX(), halfExtents.getZ()); break; } - default: + default: { - printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType); + printf("unknown Cone up axis\n"); } - } + }; - if (shape) + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; + int numSpheres = mss->m_localPositionArraySize; + + btAlignedObjectArray<btVector3> tmpPos; + btAlignedObjectArray<btScalar> radii; + radii.resize(numSpheres); + tmpPos.resize(numSpheres); + int i; + for (i = 0; i < numSpheres; i++) + { + tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); + radii[i] = mss->m_localPositionArrayPtr[i].m_radius; + } + shape = createMultiSphereShape(&tmpPos[0], &radii[0], numSpheres); + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: { - shape->setMargin(bsd->m_collisionMargin); + // int sz = sizeof(btConvexHullShapeData); + // int sz2 = sizeof(btConvexInternalShapeData); + // int sz3 = sizeof(btCollisionShapeData); + btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; + int numPoints = convexData->m_numUnscaledPoints; + + btAlignedObjectArray<btVector3> tmpPoints; + tmpPoints.resize(numPoints); + int i; + for (i = 0; i < numPoints; i++) + { +#ifdef BT_USE_DOUBLE_PRECISION + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); +#else + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); +#endif //BT_USE_DOUBLE_PRECISION + } + btConvexHullShape* hullShape = createConvexHullShape(); + for (i = 0; i < numPoints; i++) + { + hullShape->addPoint(tmpPoints[i]); + } + hullShape->setMargin(bsd->m_collisionMargin); + //hullShape->initializePolyhedralFeatures(); + shape = hullShape; + break; + } + default: + { + printf("error: cannot create shape type (%d)\n", shapeData->m_shapeType); + } + } - btVector3 localScaling; - localScaling.deSerializeFloat(bsd->m_localScaling); - shape->setLocalScaling(localScaling); + if (shape) + { + shape->setMargin(bsd->m_collisionMargin); - } - break; + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + shape->setLocalScaling(localScaling); } + break; + } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData; @@ -522,10 +494,10 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS return 0; } - btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); + btVector3 scaling; + scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); meshInterface->setScaling(scaling); - btOptimizedBvh* bvh = 0; #if 1 if (trimesh->m_quantizedFloatBvh) @@ -534,7 +506,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS if (bvhPtr && *bvhPtr) { bvh = *bvhPtr; - } else + } + else { bvh = createOptimizedBvh(); bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh); @@ -546,7 +519,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS if (bvhPtr && *bvhPtr) { bvh = *bvhPtr; - } else + } + else { bvh = createOptimizedBvh(); bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh); @@ -554,8 +528,7 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS } #endif - - btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh); + btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface, bvh); trimeshShape->setMargin(trimesh->m_collisionMargin); shape = trimeshShape; @@ -567,67 +540,66 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS #ifdef USE_INTERNAL_EDGE_UTILITY gContactAddedCallback = btAdjustInternalEdgeContactsCallback; -#endif //USE_INTERNAL_EDGE_UTILITY - +#endif //USE_INTERNAL_EDGE_UTILITY } //printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin); break; } case COMPOUND_SHAPE_PROXYTYPE: + { + btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; + btCompoundShape* compoundShape = createCompoundShape(); + + //btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0]; + + btAlignedObjectArray<btCollisionShape*> childShapes; + for (int i = 0; i < compoundData->m_numChildShapes; i++) { - btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; - btCompoundShape* compoundShape = createCompoundShape(); + //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; + btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; - btAlignedObjectArray<btCollisionShape*> childShapes; - for (int i=0;i<compoundData->m_numChildShapes;i++) + btCollisionShape* childShape = convertCollisionShape(cd); + if (childShape) + { + btTransform localTransform; + localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); + compoundShape->addChildShape(localTransform, childShape); + } + else { - btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; - - btCollisionShape* childShape = convertCollisionShape(cd); - if (childShape) - { - btTransform localTransform; - localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); - compoundShape->addChildShape(localTransform,childShape); - } else - { #ifdef _DEBUG - printf("error: couldn't create childShape for compoundShape\n"); + printf("error: couldn't create childShape for compoundShape\n"); #endif - } - } - shape = compoundShape; - - break; } + shape = compoundShape; + + break; + } case SOFTBODY_SHAPE_PROXYTYPE: - { - return 0; - } + { + return 0; + } default: - { + { #ifdef _DEBUG - printf("unsupported shape type (%d)\n",shapeData->m_shapeType); + printf("unsupported shape type (%d)\n", shapeData->m_shapeType); #endif - } } + } - return shape; - + return shape; } - - char* btCollisionWorldImporter::duplicateName(const char* name) { if (name) { int l = (int)strlen(name); - char* newName = new char[l+1]; - memcpy(newName,name,l); + char* newName = new char[l + 1]; + memcpy(newName, name, l); newName[l] = 0; m_allocatedNames.push_back(newName); return newName; @@ -635,53 +607,43 @@ char* btCollisionWorldImporter::duplicateName(const char* name) return 0; } - - - - - - - - - - -btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) +btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) { btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer(); - for (int i=0;i<meshData.m_numMeshParts;i++) + for (int i = 0; i < meshData.m_numMeshParts; i++) { btIndexedMesh meshPart; meshPart.m_numTriangles = meshData.m_meshPartsPtr[i].m_numTriangles; meshPart.m_numVertices = meshData.m_meshPartsPtr[i].m_numVertices; - if (meshData.m_meshPartsPtr[i].m_indices32) { meshPart.m_indexType = PHY_INTEGER; - meshPart.m_triangleIndexStride = 3*sizeof(int); - int* indexArray = (int*)btAlignedAlloc(sizeof(int)*3*meshPart.m_numTriangles,16); + meshPart.m_triangleIndexStride = 3 * sizeof(int); + int* indexArray = (int*)btAlignedAlloc(sizeof(int) * 3 * meshPart.m_numTriangles, 16); m_indexArrays.push_back(indexArray); - for (int j=0;j<3*meshPart.m_numTriangles;j++) + for (int j = 0; j < 3 * meshPart.m_numTriangles; j++) { indexArray[j] = meshData.m_meshPartsPtr[i].m_indices32[j].m_value; } meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; - } else + } + else { if (meshData.m_meshPartsPtr[i].m_3indices16) { meshPart.m_indexType = PHY_SHORT; - meshPart.m_triangleIndexStride = sizeof(short int)*3;//sizeof(btShortIntIndexTripletData); + meshPart.m_triangleIndexStride = sizeof(short int) * 3; //sizeof(btShortIntIndexTripletData); - short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int) * 3 * meshPart.m_numTriangles, 16); m_shortIndexArrays.push_back(indexArray); - for (int j=0;j<meshPart.m_numTriangles;j++) + for (int j = 0; j < meshPart.m_numTriangles; j++) { - indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0]; - indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1]; - indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2]; + indexArray[3 * j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0]; + indexArray[3 * j + 1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1]; + indexArray[3 * j + 2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2]; } meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; @@ -689,10 +651,10 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri if (meshData.m_meshPartsPtr[i].m_indices16) { meshPart.m_indexType = PHY_SHORT; - meshPart.m_triangleIndexStride = 3*sizeof(short int); - short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + meshPart.m_triangleIndexStride = 3 * sizeof(short int); + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int) * 3 * meshPart.m_numTriangles, 16); m_shortIndexArrays.push_back(indexArray); - for (int j=0;j<3*meshPart.m_numTriangles;j++) + for (int j = 0; j < 3 * meshPart.m_numTriangles; j++) { indexArray[j] = meshData.m_meshPartsPtr[i].m_indices16[j].m_value; } @@ -703,16 +665,16 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri if (meshData.m_meshPartsPtr[i].m_3indices8) { meshPart.m_indexType = PHY_UCHAR; - meshPart.m_triangleIndexStride = sizeof(unsigned char)*3; + meshPart.m_triangleIndexStride = sizeof(unsigned char) * 3; - unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char)*3*meshPart.m_numTriangles,16); + unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char) * 3 * meshPart.m_numTriangles, 16); m_charIndexArrays.push_back(indexArray); - for (int j=0;j<meshPart.m_numTriangles;j++) + for (int j = 0; j < meshPart.m_numTriangles; j++) { - indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0]; - indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1]; - indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2]; + indexArray[3 * j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0]; + indexArray[3 * j + 1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1]; + indexArray[3 * j + 2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2]; } meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; @@ -723,10 +685,10 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri { meshPart.m_vertexType = PHY_FLOAT; meshPart.m_vertexStride = sizeof(btVector3FloatData); - btVector3FloatData* vertices = (btVector3FloatData*) btAlignedAlloc(sizeof(btVector3FloatData)*meshPart.m_numVertices,16); + btVector3FloatData* vertices = (btVector3FloatData*)btAlignedAlloc(sizeof(btVector3FloatData) * meshPart.m_numVertices, 16); m_floatVertexArrays.push_back(vertices); - for (int j=0;j<meshPart.m_numVertices;j++) + for (int j = 0; j < meshPart.m_numVertices; j++) { vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[0]; vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[1]; @@ -734,16 +696,16 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[3]; } meshPart.m_vertexBase = (const unsigned char*)vertices; - } else + } + else { meshPart.m_vertexType = PHY_DOUBLE; meshPart.m_vertexStride = sizeof(btVector3DoubleData); - - btVector3DoubleData* vertices = (btVector3DoubleData*) btAlignedAlloc(sizeof(btVector3DoubleData)*meshPart.m_numVertices,16); + btVector3DoubleData* vertices = (btVector3DoubleData*)btAlignedAlloc(sizeof(btVector3DoubleData) * meshPart.m_numVertices, 16); m_doubleVertexArrays.push_back(vertices); - for (int j=0;j<meshPart.m_numVertices;j++) + for (int j = 0; j < meshPart.m_numVertices; j++) { vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[0]; vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[1]; @@ -755,14 +717,13 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri if (meshPart.m_triangleIndexBase && meshPart.m_vertexBase) { - meshInterface->addIndexedMesh(meshPart,meshPart.m_indexType); + meshInterface->addIndexedMesh(meshPart, meshPart.m_indexType); } } return meshInterface; } - btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData) { //create a new btStridingMeshInterfaceData that is an exact copy of shapedata and store it in the WorldImporter @@ -772,7 +733,7 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa newData->m_numMeshParts = interfaceData->m_numMeshParts; newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts]; - for(int i = 0;i < newData->m_numMeshParts;i++) + for (int i = 0; i < newData->m_numMeshParts; i++) { btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i]; btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i]; @@ -780,18 +741,18 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa curNewPart->m_numTriangles = curPart->m_numTriangles; curNewPart->m_numVertices = curPart->m_numVertices; - if(curPart->m_vertices3f) + if (curPart->m_vertices3f) { curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices]; - memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(btVector3FloatData) * curNewPart->m_numVertices); + memcpy(curNewPart->m_vertices3f, curPart->m_vertices3f, sizeof(btVector3FloatData) * curNewPart->m_numVertices); } else curNewPart->m_vertices3f = NULL; - if(curPart->m_vertices3d) + if (curPart->m_vertices3d) { curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices]; - memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices); + memcpy(curNewPart->m_vertices3d, curPart->m_vertices3d, sizeof(btVector3DoubleData) * curNewPart->m_numVertices); } else curNewPart->m_vertices3d = NULL; @@ -799,63 +760,60 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa int numIndices = curNewPart->m_numTriangles * 3; ///the m_3indices8 was not initialized in some Bullet versions, this can cause crashes at loading time ///we catch it by only dealing with m_3indices8 if none of the other indices are initialized - bool uninitialized3indices8Workaround =false; + bool uninitialized3indices8Workaround = false; - if(curPart->m_indices32) + if (curPart->m_indices32) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_indices32 = new btIntIndexData[numIndices]; - memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices); + memcpy(curNewPart->m_indices32, curPart->m_indices32, sizeof(btIntIndexData) * numIndices); } else curNewPart->m_indices32 = NULL; - if(curPart->m_3indices16) + if (curPart->m_3indices16) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles]; - memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); + memcpy(curNewPart->m_3indices16, curPart->m_3indices16, sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); } else curNewPart->m_3indices16 = NULL; - if(curPart->m_indices16) + if (curPart->m_indices16) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_indices16 = new btShortIntIndexData[numIndices]; - memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices); + memcpy(curNewPart->m_indices16, curPart->m_indices16, sizeof(btShortIntIndexData) * numIndices); } else curNewPart->m_indices16 = NULL; - if(!uninitialized3indices8Workaround && curPart->m_3indices8) + if (!uninitialized3indices8Workaround && curPart->m_3indices8) { curNewPart->m_3indices8 = new btCharIndexTripletData[curNewPart->m_numTriangles]; - memcpy(curNewPart->m_3indices8,curPart->m_3indices8,sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); + memcpy(curNewPart->m_3indices8, curPart->m_3indices8, sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); } else curNewPart->m_3indices8 = NULL; - } m_allocatedbtStridingMeshInterfaceDatas.push_back(newData); - return(newData); + return (newData); } #ifdef USE_INTERNAL_EDGE_UTILITY -extern ContactAddedCallback gContactAddedCallback; +extern ContactAddedCallback gContactAddedCallback; -static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) +static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1) { - - btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1); - //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); - //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); + btAdjustInternalEdgeContacts(cp, colObj1, colObj0, partId1, index1); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); return true; } -#endif //USE_INTERNAL_EDGE_UTILITY - +#endif //USE_INTERNAL_EDGE_UTILITY /* btRigidBody* btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName) @@ -894,29 +852,27 @@ btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char return 0; } -btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName) +btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName) { btCollisionObject* colObj = new btCollisionObject(); colObj->setWorldTransform(startTransform); colObj->setCollisionShape(shape); - m_collisionWorld->addCollisionObject(colObj);//todo: flags etc + m_collisionWorld->addCollisionObject(colObj); //todo: flags etc if (bodyName) { char* newname = duplicateName(bodyName); - m_objectNameMap.insert(colObj,newname); - m_nameColObjMap.insert(newname,colObj); + m_objectNameMap.insert(colObj, newname); + m_nameColObjMap.insert(newname, colObj); } m_allocatedCollisionObjects.push_back(colObj); return colObj; } - - -btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal,btScalar planeConstant) +btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal, btScalar planeConstant) { - btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant); + btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal, planeConstant); m_allocatedCollisionShapes.push_back(shape); return shape; } @@ -933,85 +889,83 @@ btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius) return shape; } - btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height) { - btCapsuleShapeX* shape = new btCapsuleShapeX(radius,height); + btCapsuleShapeX* shape = new btCapsuleShapeX(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } btCollisionShape* btCollisionWorldImporter::createCapsuleShapeY(btScalar radius, btScalar height) { - btCapsuleShape* shape = new btCapsuleShape(radius,height); + btCapsuleShape* shape = new btCapsuleShape(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } btCollisionShape* btCollisionWorldImporter::createCapsuleShapeZ(btScalar radius, btScalar height) { - btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius,height); + btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius, btScalar height) { - btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius)); + btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height, radius, radius)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius, btScalar height) { - btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius)); + btCylinderShape* shape = new btCylinderShape(btVector3(radius, height, radius)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius, btScalar height) { - btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height)); + btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius, radius, height)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius, btScalar height) { - btConeShapeX* shape = new btConeShapeX(radius,height); + btConeShapeX* shape = new btConeShapeX(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius, btScalar height) { - btConeShape* shape = new btConeShape(radius,height); + btConeShape* shape = new btConeShape(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius, btScalar height) { - btConeShapeZ* shape = new btConeShapeZ(radius,height); + btConeShapeZ* shape = new btConeShapeZ(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() +btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() { btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray(); m_allocatedTriangleIndexArrays.push_back(in); return in; } -btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() +btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() { btOptimizedBvh* bvh = new btOptimizedBvh(); m_allocatedBvhs.push_back(bvh); return bvh; } - btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap() { btTriangleInfoMap* tim = new btTriangleInfoMap(); @@ -1023,16 +977,15 @@ btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btS { if (bvh) { - btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false); + btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh, bvh->isQuantized(), false); bvhTriMesh->setOptimizedBvh(bvh); m_allocatedCollisionShapes.push_back(bvhTriMesh); return bvhTriMesh; } - btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true); + btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh, true); m_allocatedCollisionShapes.push_back(ts); return ts; - } btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh) { @@ -1044,9 +997,8 @@ btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshI btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh); m_allocatedCollisionShapes.push_back(shape); return shape; - } -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT btConvexHullShape* btCollisionWorldImporter::createConvexHullShape() { @@ -1062,25 +1014,22 @@ btCompoundShape* btCollisionWorldImporter::createCompoundShape() return shape; } - -btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling) +btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScaling) { - btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,localScaling); + btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape, localScaling); m_allocatedCollisionShapes.push_back(shape); return shape; } -btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres) +btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres) { btMultiSphereShape* shape = new btMultiSphereShape(positions, radi, numSpheres); m_allocatedCollisionShapes.push_back(shape); return shape; } - - - // query for data -int btCollisionWorldImporter::getNumCollisionShapes() const +// query for data +int btCollisionWorldImporter::getNumCollisionShapes() const { return m_allocatedCollisionShapes.size(); } @@ -1093,23 +1042,21 @@ btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index) btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name) { btCollisionShape** shapePtr = m_nameShapeMap.find(name); - if (shapePtr&& *shapePtr) + if (shapePtr && *shapePtr) { return *shapePtr; } return 0; } - -const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const +const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const { - const char*const * namePtr = m_objectNameMap.find(ptr); + const char* const* namePtr = m_objectNameMap.find(ptr); if (namePtr && *namePtr) return *namePtr; return 0; } - int btCollisionWorldImporter::getNumRigidBodies() const { return m_allocatedRigidBodies.size(); @@ -1120,12 +1067,11 @@ btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) cons return m_allocatedRigidBodies[index]; } - int btCollisionWorldImporter::getNumBvhs() const { return m_allocatedBvhs.size(); } - btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const +btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const { return m_allocatedBvhs[index]; } @@ -1139,5 +1085,3 @@ btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index { return m_allocatedTriangleInfoMaps[index]; } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h index 9a6d16fbea7..5e8bc95341d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_COLLISION_WORLD_IMPORTER_H #define BT_COLLISION_WORLD_IMPORTER_H @@ -26,7 +25,6 @@ class btCollisionShape; class btCollisionObject; struct btBulletSerializedArrays; - struct ConstraintInput; class btCollisionWorld; struct btCollisionShapeData; @@ -46,9 +44,6 @@ class btSliderConstraint; class btGearConstraint; struct btContactSolverInfo; - - - class btCollisionWorldImporter { protected: @@ -56,60 +51,53 @@ protected: int m_verboseMode; - btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; + btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies; - btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs; + btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs; btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps; btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays; btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas; btAlignedObjectArray<btCollisionObject*> m_allocatedCollisionObjects; + btAlignedObjectArray<char*> m_allocatedNames; - btAlignedObjectArray<char*> m_allocatedNames; + btAlignedObjectArray<int*> m_indexArrays; + btAlignedObjectArray<short int*> m_shortIndexArrays; + btAlignedObjectArray<unsigned char*> m_charIndexArrays; - btAlignedObjectArray<int*> m_indexArrays; - btAlignedObjectArray<short int*> m_shortIndexArrays; - btAlignedObjectArray<unsigned char*> m_charIndexArrays; + btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays; + btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays; - btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays; - btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays; + btHashMap<btHashPtr, btOptimizedBvh*> m_bvhMap; + btHashMap<btHashPtr, btTriangleInfoMap*> m_timMap; + btHashMap<btHashString, btCollisionShape*> m_nameShapeMap; + btHashMap<btHashString, btCollisionObject*> m_nameColObjMap; - btHashMap<btHashPtr,btOptimizedBvh*> m_bvhMap; - btHashMap<btHashPtr,btTriangleInfoMap*> m_timMap; - - btHashMap<btHashString,btCollisionShape*> m_nameShapeMap; - btHashMap<btHashString,btCollisionObject*> m_nameColObjMap; - - btHashMap<btHashPtr,const char*> m_objectNameMap; - - btHashMap<btHashPtr,btCollisionShape*> m_shapeMap; - btHashMap<btHashPtr,btCollisionObject*> m_bodyMap; + btHashMap<btHashPtr, const char*> m_objectNameMap; + btHashMap<btHashPtr, btCollisionShape*> m_shapeMap; + btHashMap<btHashPtr, btCollisionObject*> m_bodyMap; //methods + char* duplicateName(const char* name); - - char* duplicateName(const char* name); - - btCollisionShape* convertCollisionShape( btCollisionShapeData* shapeData ); - + btCollisionShape* convertCollisionShape(btCollisionShapeData* shapeData); public: - btCollisionWorldImporter(btCollisionWorld* world); virtual ~btCollisionWorldImporter(); - bool convertAllObjects( btBulletSerializedArrays* arrays); + bool convertAllObjects(btBulletSerializedArrays* arrays); - ///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load. + ///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load. ///make sure you don't use the dynamics world containing objects after you call this method virtual void deleteAllData(); - void setVerboseMode(int verboseMode) + void setVerboseMode(int verboseMode) { m_verboseMode = verboseMode; } @@ -119,15 +107,14 @@ public: return m_verboseMode; } - // query for data - int getNumCollisionShapes() const; + // query for data + int getNumCollisionShapes() const; btCollisionShape* getCollisionShapeByIndex(int index); int getNumRigidBodies() const; btCollisionObject* getRigidBodyByIndex(int index) const; - int getNumConstraints() const; int getNumBvhs() const; - btOptimizedBvh* getBvhByIndex(int index) const; + btOptimizedBvh* getBvhByIndex(int index) const; int getNumTriangleInfoMaps() const; btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const; @@ -135,56 +122,48 @@ public: btCollisionShape* getCollisionShapeByName(const char* name); btCollisionObject* getCollisionObjectByName(const char* name); - - const char* getNameForPointer(const void* ptr) const; + const char* getNameForPointer(const void* ptr) const; ///those virtuals are called by load and can be overridden by the user - - //bodies - virtual btCollisionObject* createCollisionObject( const btTransform& startTransform, btCollisionShape* shape,const char* bodyName); + virtual btCollisionObject* createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName); ///shapes - virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal, btScalar planeConstant); virtual btCollisionShape* createBoxShape(const btVector3& halfExtents); virtual btCollisionShape* createSphereShape(btScalar radius); virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height); virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height); virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height); - virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height); - virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height); - virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height); - virtual class btTriangleIndexVertexArray* createTriangleMeshContainer(); - virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh); + virtual btCollisionShape* createCylinderShapeX(btScalar radius, btScalar height); + virtual btCollisionShape* createCylinderShapeY(btScalar radius, btScalar height); + virtual btCollisionShape* createCylinderShapeZ(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeX(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeY(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeZ(btScalar radius, btScalar height); + virtual class btTriangleIndexVertexArray* createTriangleMeshContainer(); + virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh); virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh); #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh); -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData); virtual class btConvexHullShape* createConvexHullShape(); virtual class btCompoundShape* createCompoundShape(); - virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape); + virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScalingbtBvhTriangleMeshShape); - virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres); + virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres); virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData); ///acceleration and connectivity structures - virtual btOptimizedBvh* createOptimizedBvh(); + virtual btOptimizedBvh* createOptimizedBvh(); virtual btTriangleInfoMap* createTriangleInfoMap(); - - - - }; - -#endif //BT_WORLD_IMPORTER_H +#endif //BT_WORLD_IMPORTER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 589a7f55f2b..b5f4a3c869f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -25,56 +25,58 @@ subject to the following restrictions: btShapePairCallback gCompoundChildShapePairCallback = 0; -btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_isSwapped(isSwapped), -m_sharedManifold(ci.m_manifold) +btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_isSwapped(isSwapped), + m_sharedManifold(ci.m_manifold) { m_ownsManifold = false; - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); - + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + btAssert(colObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); m_compoundShapeRevision = compoundShape->getUpdateRevision(); - - - preallocateChildAlgorithms(body0Wrap,body1Wrap); + + preallocateChildAlgorithms(body0Wrap, body1Wrap); } -void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) +void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); - + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap; + btAssert(colObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); int numChildren = compoundShape->getNumChildShapes(); int i; - + m_childCollisionAlgorithms.resize(numChildren); - for (i=0;i<numChildren;i++) + for (i = 0; i < numChildren; i++) { if (compoundShape->getDynamicAabbTree()) { m_childCollisionAlgorithms[i] = 0; - } else + } + else { - const btCollisionShape* childShape = compoundShape->getChildShape(i); - btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully) - m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold); + btCollisionObjectWrapper childWrap(colObjWrap, childShape, colObjWrap->getCollisionObject(), colObjWrap->getWorldTransform(), -1, i); //wrong child trans, but unused (hopefully) + m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap, otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact; + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints; } } } -void btCompoundCollisionAlgorithm::removeChildAlgorithms() +void btCompoundCollisionAlgorithm::removeChildAlgorithms() { int numChildren = m_childCollisionAlgorithms.size(); int i; - for (i=0;i<numChildren;i++) + for (i = 0; i < numChildren; i++) { if (m_childCollisionAlgorithms[i]) { @@ -89,66 +91,79 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() removeChildAlgorithms(); } - - - -struct btCompoundLeafCallback : btDbvt::ICollide +struct btCompoundLeafCallback : btDbvt::ICollide { - public: - const btCollisionObjectWrapper* m_compoundColObjWrap; const btCollisionObjectWrapper* m_otherObjWrap; btDispatcher* m_dispatcher; const btDispatcherInfo& m_dispatchInfo; - btManifoldResult* m_resultOut; - btCollisionAlgorithm** m_childCollisionAlgorithms; - btPersistentManifold* m_sharedManifold; - - btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold) - :m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithms(childCollisionAlgorithms), - m_sharedManifold(sharedManifold) - { + btManifoldResult* m_resultOut; + btCollisionAlgorithm** m_childCollisionAlgorithms; + btPersistentManifold* m_sharedManifold; + btCompoundLeafCallback(const btCollisionObjectWrapper* compoundObjWrap, const btCollisionObjectWrapper* otherObjWrap, btDispatcher* dispatcher, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut, btCollisionAlgorithm** childCollisionAlgorithms, btPersistentManifold* sharedManifold) + : m_compoundColObjWrap(compoundObjWrap), m_otherObjWrap(otherObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithms(childCollisionAlgorithms), m_sharedManifold(sharedManifold) + { } - - void ProcessChildShape(const btCollisionShape* childShape,int index) + void ProcessChildShape(const btCollisionShape* childShape, int index) { - btAssert(index>=0); + btAssert(index >= 0); const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape()); - btAssert(index<compoundShape->getNumChildShapes()); + btAssert(index < compoundShape->getNumChildShapes()); + if (gCompoundChildShapePairCallback) + { + if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) + return; + } //backup - btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); - + btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(index); - btTransform newChildWorldTrans = orgTrans*childTrans ; + btTransform newChildWorldTrans = orgTrans * childTrans; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); - m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + btVector3 aabbMin0, aabbMax0; + childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0); - if (gCompoundChildShapePairCallback) - { - if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) - return; - } + btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= extendAabb; + aabbMax0 += extendAabb; - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) - { + btVector3 aabbMin1, aabbMax1; + m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1); - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); + if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) + { + btTransform preTransform = childTrans; + if (this->m_compoundColObjWrap->m_preTransform) + { + preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform)); + } + btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index); + + btCollisionAlgorithm* algo = 0; + bool allocatedAlgorithm = false; - //the contactpoint is still projected back using the original inverted worldtrans - if (!m_childCollisionAlgorithms[index]) - m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold); + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + allocatedAlgorithm = true; + } + else + { + //the contactpoint is still projected back using the original inverted worldtrans + if (!m_childCollisionAlgorithms[index]) + { + m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + } + algo = m_childCollisionAlgorithms[index]; + } - const btCollisionObjectWrapper* tmpWrap = 0; ///detect swapping case @@ -156,16 +171,16 @@ public: { tmpWrap = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersA(-1,index); - } else + m_resultOut->setShapeIdentifiersA(-1, index); + } + else { tmpWrap = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersB(-1,index); + m_resultOut->setShapeIdentifiersB(-1, index); } - - m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); + algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut); #if 0 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) @@ -179,14 +194,19 @@ public: if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - + if (allocatedAlgorithm) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } } } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { int index = leaf->dataAsInt; @@ -203,22 +223,16 @@ public: } #endif - ProcessChildShape(childShape,index); - + ProcessChildShape(childShape, index); } }; - - - - - -void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); + btAssert(colObjWrap->getCollisionShape()->isCompound()); const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); ///btCompoundShape might have changed: @@ -227,36 +241,36 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap { ///clear and update all removeChildAlgorithms(); - - preallocateChildAlgorithms(body0Wrap,body1Wrap); + + preallocateChildAlgorithms(body0Wrap, body1Wrap); m_compoundShapeRevision = compoundShape->getUpdateRevision(); } - if (m_childCollisionAlgorithms.size()==0) + if (m_childCollisionAlgorithms.size() == 0) return; - + const btDbvt* tree = compoundShape->getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps - btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); + btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold); ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; - btManifoldArray manifoldArray; - for (i=0;i<m_childCollisionAlgorithms.size();i++) + manifoldArray.resize(0); + for (i = 0; i < m_childCollisionAlgorithms.size(); i++) { if (m_childCollisionAlgorithms[i]) { m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); - for (int m=0;m<manifoldArray.size();m++) + for (int m = 0; m < manifoldArray.size(); m++) { if (manifoldArray[m]->getNumContacts()) { resultOut->setPersistentManifold(manifoldArray[m]); resultOut->refreshContactPoints(); - resultOut->setPersistentManifold(0);//??necessary? + resultOut->setPersistentManifold(0); //??necessary? } } manifoldArray.resize(0); @@ -266,54 +280,56 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap if (tree) { - - btVector3 localAabbMin,localAabbMax; + btVector3 localAabbMin, localAabbMax; btTransform otherInCompoundSpace; otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); - otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax); + btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + localAabbMin -= extraExtends; + localAabbMax += extraExtends; - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(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 + tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, 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++) + for (i = 0; i < numChildren; i++) { - callback.ProcessChildShape(compoundShape->getChildShape(i),i); + callback.ProcessChildShape(compoundShape->getChildShape(i), i); } } { - //iterate over all children, perform an AABB check inside ProcessChildShape + //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.size(); int i; - btManifoldArray manifoldArray; - const btCollisionShape* childShape = 0; - btTransform orgTrans; - - btTransform newChildWorldTrans; - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;i<numChildren;i++) + manifoldArray.resize(0); + const btCollisionShape* childShape = 0; + btTransform orgTrans; + + btTransform newChildWorldTrans; + btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; + + for (i = 0; i < numChildren; i++) { if (m_childCollisionAlgorithms[i]) { childShape = compoundShape->getChildShape(i); - //if not longer overlapping, remove the algorithm + //if not longer overlapping, remove the algorithm orgTrans = colObjWrap->getWorldTransform(); - + const btTransform& childTrans = compoundShape->getChildTransform(i); - newChildWorldTrans = orgTrans*childTrans ; + newChildWorldTrans = orgTrans * childTrans; //perform an AABB check first - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); - otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0); + otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1); - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); @@ -324,15 +340,15 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap } } -btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { btAssert(0); //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures - btCollisionObject* colObj = m_isSwapped? body1 : body0; - btCollisionObject* otherObj = m_isSwapped? body0 : body1; + btCollisionObject* colObj = m_isSwapped ? body1 : body0; + btCollisionObject* otherObj = m_isSwapped ? body0 : body1; + + btAssert(colObj->getCollisionShape()->isCompound()); - btAssert (colObj->getCollisionShape()->isCompound()); - btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps @@ -346,33 +362,29 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* int numChildren = m_childCollisionAlgorithms.size(); int i; - btTransform orgTrans; - btScalar frac; - for (i=0;i<numChildren;i++) + btTransform orgTrans; + btScalar frac; + for (i = 0; i < numChildren; i++) { //btCollisionShape* childShape = compoundShape->getChildShape(i); //backup - orgTrans = colObj->getWorldTransform(); - + orgTrans = colObj->getWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(i); //btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( orgTrans*childTrans ); + colObj->setWorldTransform(orgTrans * childTrans); //btCollisionShape* tmpShape = colObj->getCollisionShape(); //colObj->internalSetTemporaryCollisionShape( childShape ); - frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); - if (frac<hitFraction) + frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut); + if (frac < hitFraction) { hitFraction = frac; } //revert back //colObj->internalSetTemporaryCollisionShape( tmpShape); - colObj->setWorldTransform( orgTrans); + colObj->setWorldTransform(orgTrans); } return hitFraction; - } - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 7d792c18d34..4ea5e77185b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -26,6 +26,7 @@ class btDispatcher; #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" class btDispatcher; class btCollisionObject; @@ -34,67 +35,65 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol extern btShapePairCallback gCompoundChildShapePairCallback; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm +class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { + btNodeStack stack2; + btManifoldArray manifoldArray; + protected: btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; - class btPersistentManifold* m_sharedManifold; - bool m_ownsManifold; + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; + int m_compoundShapeRevision; //to keep track of changes, so that childAlgorithm array can be updated - int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated - - void removeChildAlgorithms(); - - void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + void removeChildAlgorithms(); -public: + void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); - btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); +public: + btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btCompoundCollisionAlgorithm(); - btCollisionAlgorithm* getChildAlgorithm (int n) const + btCollisionAlgorithm* getChildAlgorithm(int n) const { return m_childCollisionAlgorithms[n]; } + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { int i; - for (i=0;i<m_childCollisionAlgorithms.size();i++) + for (i = 0; i < m_childCollisionAlgorithms.size(); i++) { if (m_childCollisionAlgorithms[i]) m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); - return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); - return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 1d64d84b87b..044b60dbb1d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: */ #include "btCompoundCompoundCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" @@ -23,32 +24,30 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btManifoldResult.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT +#define USE_LOCAL_STACK 1 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; -btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) +btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, isSwapped) { - - void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); - m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); + void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache), 16); + m_childCollisionAlgorithmCache = new (ptr) btHashedSimplePairCache(); const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + btAssert(col0ObjWrap->getCollisionShape()->isCompound()); const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); - + btAssert(col1ObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - - } - btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() { removeChildAlgorithms(); @@ -56,32 +55,30 @@ btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() btAlignedFree(m_childCollisionAlgorithmCache); } -void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { int i; btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;i<pairs.size();i++) + for (i = 0; i < pairs.size(); i++) { if (pairs[i].m_userPointer) { - ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray); } } } - -void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() +void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() { btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); int numChildren = pairs.size(); int i; - for (i=0;i<numChildren;i++) + for (i = 0; i < numChildren; i++) { if (pairs[i].m_userPointer) { - btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; algo->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algo); } @@ -89,106 +86,106 @@ void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() m_childCollisionAlgorithmCache->removeAllPairs(); } -struct btCompoundCompoundLeafCallback : btDbvt::ICollide +struct btCompoundCompoundLeafCallback : btDbvt::ICollide { int m_numOverlapPairs; - const btCollisionObjectWrapper* m_compound0ColObjWrap; const btCollisionObjectWrapper* m_compound1ColObjWrap; btDispatcher* m_dispatcher; const btDispatcherInfo& m_dispatchInfo; - btManifoldResult* m_resultOut; - - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; - - btPersistentManifold* m_sharedManifold; - - btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, - const btCollisionObjectWrapper* compound0ObjWrap, - btDispatcher* dispatcher, - const btDispatcherInfo& dispatchInfo, - btManifoldResult* resultOut, - btHashedSimplePairCache* childAlgorithmsCache, - btPersistentManifold* sharedManifold) - :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithmCache(childAlgorithmsCache), - m_sharedManifold(sharedManifold) - { + btManifoldResult* m_resultOut; - } + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + btPersistentManifold* m_sharedManifold; + btCompoundCompoundLeafCallback(const btCollisionObjectWrapper* compound1ObjWrap, + const btCollisionObjectWrapper* compound0ObjWrap, + btDispatcher* dispatcher, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut, + btHashedSimplePairCache* childAlgorithmsCache, + btPersistentManifold* sharedManifold) + : m_numOverlapPairs(0), m_compound0ColObjWrap(compound1ObjWrap), m_compound1ColObjWrap(compound0ObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithmCache(childAlgorithmsCache), m_sharedManifold(sharedManifold) + { + } - - void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) + void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1) { + BT_PROFILE("btCompoundCompoundLeafCallback::Process"); m_numOverlapPairs++; - int childIndex0 = leaf0->dataAsInt; int childIndex1 = leaf1->dataAsInt; - - - btAssert(childIndex0>=0); - btAssert(childIndex1>=0); + btAssert(childIndex0 >= 0); + btAssert(childIndex1 >= 0); const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape()); - btAssert(childIndex0<compoundShape0->getNumChildShapes()); + btAssert(childIndex0 < compoundShape0->getNumChildShapes()); const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape()); - btAssert(childIndex1<compoundShape1->getNumChildShapes()); + btAssert(childIndex1 < compoundShape1->getNumChildShapes()); const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); //backup - btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); + btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); - btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; - - btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); + btTransform newChildWorldTrans0 = orgTrans0 * childTrans0; + + btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); - btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; - + btTransform newChildWorldTrans1 = orgTrans1 * childTrans1; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); - + btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; + childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0); + childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1); + + btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; + if (gCompoundCompoundChildShapePairCallback) { - if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) + if (!gCompoundCompoundChildShapePairCallback(childShape0, childShape1)) return; } - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { - btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); - btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); - - - btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); + btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap->getCollisionObject(), newChildWorldTrans0, -1, childIndex0); + btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap->getCollisionObject(), newChildWorldTrans1, -1, childIndex1); + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1); + bool removePair = false; btCollisionAlgorithm* colAlgo = 0; - - if (pair) + if (m_resultOut->m_closestPointDistanceThreshold > 0) { - colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - - } else + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); + removePair = true; + } + else { - colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); - pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); - btAssert(pair); - pair->m_userPointer = colAlgo; + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; + } + else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } } btAssert(colAlgo); - + const btCollisionObjectWrapper* tmpWrap0 = 0; const btCollisionObjectWrapper* tmpWrap1 = 0; @@ -198,94 +195,100 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide m_resultOut->setBody0Wrap(&compoundWrap0); m_resultOut->setBody1Wrap(&compoundWrap1); - m_resultOut->setShapeIdentifiersA(-1,childIndex0); - m_resultOut->setShapeIdentifiersB(-1,childIndex1); + m_resultOut->setShapeIdentifiersA(-1, childIndex0); + m_resultOut->setShapeIdentifiersB(-1, childIndex1); + colAlgo->processCollision(&compoundWrap0, &compoundWrap1, m_dispatchInfo, m_resultOut); - colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); - m_resultOut->setBody0Wrap(tmpWrap0); m_resultOut->setBody1Wrap(tmpWrap1); - - + if (removePair) + { + colAlgo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(colAlgo); + } } } }; - -static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, const btTransform& xform) +static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm& a, + const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold) { - btVector3 newmin,newmax; - btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); - btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); - return Intersect(a,newb); + btVector3 newmin, newmax; + btTransformAabb(b.Mins(), b.Maxs(), 0.f, xform, newmin, newmax); + newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold); + newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold); + btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin, newmax); + return Intersect(a, newb); } - -static inline void MycollideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - btCompoundCompoundLeafCallback* callback) +static inline void MycollideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold) { - - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = btDbvt::DOUBLE_STACKSIZE - 4; + btAlignedObjectArray<btDbvt::sStkNN> stkStack; +#ifdef USE_LOCAL_STACK + ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]); + stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE); +#else + stkStack.resize(btDbvt::DOUBLE_STACKSIZE); +#endif + stkStack[0] = btDbvt::sStkNN(root0, root1); + do { - int depth=1; - int treshold=btDbvt::DOUBLE_STACKSIZE-4; - btAlignedObjectArray<btDbvt::sStkNN> stkStack; - stkStack.resize(btDbvt::DOUBLE_STACKSIZE); - stkStack[0]=btDbvt::sStkNN(root0,root1); - do { - btDbvt::sStkNN p=stkStack[--depth]; - if(MyIntersect(p.a->volume,p.b->volume,xform)) + btDbvt::sStkNN p = stkStack[--depth]; + if (MyIntersect(p.a->volume, p.b->volume, xform, distanceThreshold)) + { + if (depth > treshold) { - if(depth>treshold) + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a->isinternal()) + { + if (p.b->isinternal()) + { + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[1]); + } + else { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b); } - if(p.a->isinternal()) + } + else + { + if (p.b->isinternal()) { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); - } + stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[1]); } else { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); - } - else - { - callback->Process(p.a,p.b); - } + callback->Process(p.a, p.b); } } - } while(depth); - } + } + } while (depth); + } } -void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btCompoundCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; + const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + btAssert(col0ObjWrap->getCollisionShape()->isCompound()); + btAssert(col1ObjWrap->getCollisionShape()->isCompound()); const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); @@ -293,7 +296,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); if (!tree0 || !tree1) { - return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); + return btCompoundCollisionAlgorithm::processCollision(body0Wrap, body1Wrap, dispatchInfo, resultOut); } ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid @@ -303,24 +306,26 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb removeChildAlgorithms(); m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - } - ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; btManifoldArray manifoldArray; +#ifdef USE_LOCAL_STACK + btPersistentManifold localManifolds[4]; + manifoldArray.initializeFromBuffer(&localManifolds, 0, 4); +#endif btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;i<pairs.size();i++) + for (i = 0; i < pairs.size(); i++) { if (pairs[i].m_userPointer) { - btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; algo->getAllContactManifolds(manifoldArray); - for (int m=0;m<manifoldArray.size();m++) + for (int m = 0; m < manifoldArray.size(); m++) { if (manifoldArray[m]->getNumContacts()) { @@ -334,93 +339,75 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb } } + btCompoundCompoundLeafCallback callback(col0ObjWrap, col1ObjWrap, this->m_dispatcher, dispatchInfo, resultOut, this->m_childCollisionAlgorithmCache, m_sharedManifold); - - - btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); - - - const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); - MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); + const btTransform xform = col0ObjWrap->getWorldTransform().inverse() * col1ObjWrap->getWorldTransform(); + MycollideTT(tree0->m_root, tree1->m_root, xform, &callback, resultOut->m_closestPointDistanceThreshold); //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); //remove non-overlapping child pairs { - btAssert(m_removePairs.size()==0); + btAssert(m_removePairs.size() == 0); //iterate over all children, perform an AABB check inside ProcessChildShape btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - + int i; - btManifoldArray manifoldArray; - - - - - - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;i<pairs.size();i++) + btManifoldArray manifoldArray; + + btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; + + for (i = 0; i < pairs.size(); i++) { if (pairs[i].m_userPointer) { btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; { - btTransform orgTrans0; const btCollisionShape* childShape0 = 0; - - btTransform newChildWorldTrans0; - btTransform orgInterpolationTrans0; + + btTransform newChildWorldTrans0; childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); - orgTrans0 = col0ObjWrap->getWorldTransform(); - orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = orgTrans0*childTrans0 ; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + newChildWorldTrans0 = col0ObjWrap->getWorldTransform() * childTrans0; + childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0); } - + btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; { - btTransform orgInterpolationTrans1; const btCollisionShape* childShape1 = 0; - btTransform orgTrans1; - btTransform newChildWorldTrans1; + btTransform newChildWorldTrans1; childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); - orgTrans1 = col1ObjWrap->getWorldTransform(); - orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = orgTrans1*childTrans1 ; - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1; + childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1); } - - - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + aabbMin1 -= thresholdVec; + aabbMax1 += thresholdVec; + + if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { algo->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algo); - m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); + m_removePairs.push_back(btSimplePair(pairs[i].m_indexA, pairs[i].m_indexB)); } } } - for (int i=0;i<m_removePairs.size();i++) + for (int i = 0; i < m_removePairs.size(); i++) { - m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); + m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA, m_removePairs[i].m_indexB); } m_removePairs.clear(); } - } -btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { btAssert(0); return 0.f; - } - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index 06a762f209d..a940d840e0c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -33,57 +33,50 @@ class btDispatcher; class btCollisionObject; class btCollisionShape; -typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); + extern btShapePairCallback gCompoundCompoundChildShapePairCallback; /// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes -class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm +class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm { - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; btSimplePairArray m_removePairs; + int m_compoundShapeRevision0; //to keep track of changes, so that childAlgorithm array can be updated + int m_compoundShapeRevision1; - int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated - int m_compoundShapeRevision1; - - void removeChildAlgorithms(); - -// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + void removeChildAlgorithms(); -public: + // void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); +public: + btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btCompoundCompoundCollisionAlgorithm(); - + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp index 1cb3d2e7a14..9087f843985 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -22,7 +22,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" - #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -34,8 +33,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -45,31 +42,28 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) { m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } -btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() -{ +btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() +{ } -btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_simplexSolver(simplexSolver), -m_pdSolver(pdSolver), -m_ownManifold (false), -m_manifoldPtr(mf), -m_lowLevelOfDetail(false) +btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_simplexSolver(simplexSolver), + m_pdSolver(pdSolver), + m_ownManifold(false), + m_manifoldPtr(mf), + m_lowLevelOfDetail(false) { (void)body0Wrap; (void)body1Wrap; } - - - btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() { if (m_ownManifold) @@ -79,26 +73,22 @@ btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() } } -void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) { m_lowLevelOfDetail = useLowLevel; } - - extern btScalar gContactBreakingThreshold; - // // Convex-Convex collision algorithm // -void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvex2dConvex2dAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - if (!m_manifoldPtr) { //swapped? - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); @@ -106,49 +96,41 @@ void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrap //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); - const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); - btVector3 normalOnB; - btVector3 pointOnBWorld; + btVector3 normalOnB; + btVector3 pointOnBWorld; { - - btGjkPairDetector::ClosestPointInput input; - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); + btGjkPairDetector gjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); { input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); - btVector3 v0,v1; + btVector3 v0, v1; btVector3 sepNormalWorldSpace; - } if (m_ownManifold) { resultOut->refreshContactPoints(); } - } - - - -btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -158,7 +140,6 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c ///col0->m_worldTransform, btScalar resultFraction = btScalar(1.); - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); @@ -166,77 +147,65 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; - //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - /// Convex0 against sphere for Convex1 { btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } - - - - } /// Sphere (for convex0) against Convex1 { btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape()); - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } } return resultFraction; - } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h index 24d13367786..9fca463fbe4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -23,70 +23,61 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil class btConvexPenetrationDepthSolver; - ///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape ///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm { - btSimplexSolverInterface* m_simplexSolver; + btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_pdSolver; - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - -public: - - btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; +public: + btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvex2dConvex2dAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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) + 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); - void setLowLevelOfDetail(bool useLowLevel); - - - const btPersistentManifold* getManifold() + const btPersistentManifold* getManifold() { return m_manifoldPtr; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + btSimplexSolverInterface* m_simplexSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + virtual ~CreateFunc(); - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm)); - return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvex2dConvex2dAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_simplexSolver, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } }; - - }; -#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H +#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 912a5285568..e50f85e2bbe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConvexConcaveCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -26,11 +26,12 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h" -btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_isSwapped(isSwapped), -m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) +btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped), + m_isSwapped(isSwapped) { } @@ -38,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() { } -void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_btConvexTriangleCallback.m_manifoldPtr) { @@ -46,55 +47,46 @@ void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& } } - -btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): - m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) +btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) { - m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap; - m_triBodyWrap = isSwapped? body0Wrap:body1Wrap; - - // - // create the manifold from the dispatcher 'manifold pool' - // - m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject()); + m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap; + m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap; + + // + // create the manifold from the dispatcher 'manifold pool' + // + m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject()); - clearCache(); + clearCache(); } btConvexTriangleCallback::~btConvexTriangleCallback() { clearCache(); - m_dispatcher->releaseManifold( m_manifoldPtr ); - + m_dispatcher->releaseManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::clearCache() +void btConvexTriangleCallback::clearCache() { m_dispatcher->clearManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::processTriangle(btVector3* triangle,int -partId, int triangleIndex) +void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("btConvexTriangleCallback::processTriangle"); if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) { return; } - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - - + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - - #if 0 ///debug drawing of the overlapping triangles @@ -108,52 +100,55 @@ partId, int triangleIndex) m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); } #endif - + if (m_convexBodyWrap->getCollisionShape()->isConvex()) { - btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + btTriangleShape tm(triangle[0], triangle[1], triangle[2]); tm.setMargin(m_collisionMarginTriangle); - - - btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr); + btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform? + btCollisionAlgorithm* colAlgo = 0; + + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS); + } const btCollisionObjectWrapper* tmpWrap = 0; if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { tmpWrap = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersA(partId,triangleIndex); + m_resultOut->setShapeIdentifiersA(partId, triangleIndex); } else { tmpWrap = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersB(partId,triangleIndex); + m_resultOut->setShapeIdentifiersB(partId, triangleIndex); } - - colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut); + + colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut); if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - - colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); } - } - - -void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) { m_convexBodyWrap = convexBodyWrap; m_triBodyWrap = triBodyWrap; @@ -167,65 +162,120 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape()); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle; - btVector3 extra(extraMargin,extraMargin,extraMargin); + convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax); + btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold; + + btVector3 extra(extraMargin, extraMargin, extraMargin); m_aabbMax += extra; m_aabbMin -= extra; - } void btConvexConcaveCollisionAlgorithm::clearCache() { m_btConvexTriangleCallback.clearCache(); - } -void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - - + BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); + const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; if (triBodyWrap->getCollisionShape()->isConcave()) { + if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) + { + btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); + btAlignedObjectArray<btVector3> queryVertices; + + if (convex->isPolyhedral()) + { + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < poly->getNumVertices(); v++) + { + btVector3 vtx; + poly->getVertex(v, vtx); + queryVertices.push_back(vtx); + } + } + btScalar maxDist = SIMD_EPSILON; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + queryVertices.push_back(btVector3(0, 0, 0)); + btSphereShape* sphere = (btSphereShape*)convex; + maxDist = sphere->getRadius() + SIMD_EPSILON; + } + if (queryVertices.size()) + { + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); + + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < queryVertices.size(); v++) + { + const btVector3& vtx = queryVertices[v]; + btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx; + btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace); + + btVector3 normalLocal; + btScalar dist; + if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal)) + { + if (dist <= maxDist) + { + normalLocal.safeNormalize(); + btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + btSphereShape* sphere = (btSphereShape*)convex; + dist -= sphere->getRadius(); + vtxWorldSpace -= sphere->getRadius() * normal; + } + resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist); + } + } + } + resultOut->refreshContactPoints(); + } + } + } + else + { + const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape()); + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btScalar collisionMarginTriangle = concaveShape->getMargin(); - - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape()); - - if (convexBodyWrap->getCollisionShape()->isConvex()) - { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut); - m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject()); - concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); - - resultOut->refreshContactPoints(); + concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax()); - m_btConvexTriangleCallback.clearWrapperData(); - + resultOut->refreshContactPoints(); + + m_btConvexTriangleCallback.clearWrapperData(); + } } - } - } - -btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) //only perform CCD above a certain threshold, this prevents blocking on the long run @@ -244,27 +294,26 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - struct LocalTriangleSphereCastCallback : public btTriangleCallback + struct LocalTriangleSphereCastCallback : public btTriangleCallback { btTransform m_ccdSphereFromTrans; btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; + btTransform m_meshTransform; - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { + LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction) + : m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { } - - + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("processTriangle"); (void)partId; (void)triangleIndex; //do a swept sphere for now @@ -272,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj ident.setIdentity(); btConvexCast::CastResult castResult; castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + btSphereShape pointShape(m_ccdSphereRadius); + btTriangleShape triShape(triangle[0], triangle[1], triangle[2]); + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); //local space? - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, + ident, ident, castResult)) { if (m_hitFraction > castResult.m_fraction) m_hitFraction = castResult.m_fraction; } - } - }; - - - - if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -302,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); + btScalar curHitFraction = btScalar(1.); //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal, + convexbody->getCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody->getHitFraction(); btCollisionObject* concavebody = triBody; - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - + btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape(); + if (triangleMesh) { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax); } - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) { - convexbody->setHitFraction( raycastCallback.m_hitFraction); + convexbody->setHitFraction(raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } return btScalar(1.); - } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index e90d06eb191..b72e4029811 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -26,38 +26,40 @@ class btDispatcher; #include "btCollisionCreateFunc.h" ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. -class btConvexTriangleCallback : public btTriangleCallback +ATTRIBUTE_ALIGNED16(class) +btConvexTriangleCallback : public btTriangleCallback { + btVector3 m_aabbMin; + btVector3 m_aabbMax; + const btCollisionObjectWrapper* m_convexBodyWrap; const btCollisionObjectWrapper* m_triBodyWrap; - btVector3 m_aabbMin; - btVector3 m_aabbMax ; - - btManifoldResult* m_resultOut; - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; - + public: -int m_triangleCount; - - btPersistentManifold* m_manifoldPtr; + BT_DECLARE_ALIGNED_ALLOCATOR(); + + int m_triangleCount; - btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btPersistentManifold* m_manifoldPtr; - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut); + btConvexTriangleCallback(btDispatcher * dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); - void clearWrapperData() + void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut); + + void clearWrapperData() { m_convexBodyWrap = 0; m_triBodyWrap = 0; } virtual ~btConvexTriangleCallback(); - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - + virtual void processTriangle(btVector3 * triangle, int partId, int triangleIndex); + void clearCache(); SIMD_FORCE_INLINE const btVector3& getAabbMin() const @@ -68,54 +70,48 @@ int m_triangleCount; { return m_aabbMax; } - }; - - - /// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm +ATTRIBUTE_ALIGNED16(class) +btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm { - - bool m_isSwapped; - btConvexTriangleCallback m_btConvexTriangleCallback; - + bool m_isSwapped; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); - btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btConvexConcaveCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject * body0, btCollisionObject * body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray & manifoldArray); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - void clearCache(); + void clearCache(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); - return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); - return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 7f2722aa463..b48d97f2b28 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: ///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums ///with reproduction case -//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 +//#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 //#define ZERO_MARGIN #include "btConvexConvexAlgorithm.h" @@ -28,8 +28,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" - - +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -42,8 +41,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -56,8 +53,6 @@ subject to the following restrictions: /////////// - - static SIMD_FORCE_INLINE void segmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, @@ -65,43 +60,49 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints( btScalar& tA, btScalar& tB, const btVector3& translation, const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) + const btVector3& dirB, btScalar hlenB) { // compute the parameters of the closest points on each line segment - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); + btScalar dirA_dot_dirB = btDot(dirA, dirB); + btScalar dirA_dot_trans = btDot(dirA, translation); + btScalar dirB_dot_trans = btDot(dirB, translation); btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - if ( denom == 0.0f ) { + if (denom == 0.0f) + { tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) + } + else + { + tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom; + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } tB = tA * dirA_dot_dirB - dirB_dot_trans; - if ( tB < -hlenB ) { + if (tB < -hlenB) + { tB = -hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; - } else if ( tB > hlenB ) { + } + else if (tB > hlenB) + { tB = hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } @@ -113,19 +114,18 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints( ptsVector = translation - offsetA + offsetB; } - static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( btVector3& normalOnB, btVector3& pointOnB, btScalar capsuleLengthA, - btScalar capsuleRadiusA, + btScalar capsuleRadiusA, btScalar capsuleLengthB, - btScalar capsuleRadiusB, + btScalar capsuleRadiusB, int capsuleAxisA, int capsuleAxisB, const btTransform& transformA, const btTransform& transformB, - btScalar distanceThreshold ) + btScalar distanceThreshold) { btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); btVector3 translationA = transformA.getOrigin(); @@ -138,80 +138,66 @@ static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( // compute the closest points of the capsule line segments - btVector3 ptsVector; // the vector between the closest points - - btVector3 offsetA, offsetB; // offsets from segment centers to their closest points - btScalar tA, tB; // parameters on line segment + btVector3 ptsVector; // the vector between the closest points + + btVector3 offsetA, offsetB; // offsets from segment centers to their closest points + btScalar tA, tB; // parameters on line segment - segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, - directionA, capsuleLengthA, directionB, capsuleLengthB ); + segmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation, + directionA, capsuleLengthA, directionB, capsuleLengthB); btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; - if ( distance > distanceThreshold ) + if (distance > distanceThreshold) return distance; btScalar lenSqr = ptsVector.length2(); - if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) { //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' btVector3 q; - btPlaneSpace1(directionA,normalOnB,q); - } else + btPlaneSpace1(directionA, normalOnB, q); + } + else { // compute the contact normal - normalOnB = ptsVector*-btRecipSqrt(lenSqr); + normalOnB = ptsVector * -btRecipSqrt(lenSqr); } - pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB; + pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB; return distance; } - - - - - - ////////// - - - - -btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver) { m_numPerturbationIterations = 0; m_minimumPointsPerturbationThreshold = 3; - m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } -btConvexConvexAlgorithm::CreateFunc::~CreateFunc() -{ +btConvexConvexAlgorithm::CreateFunc::~CreateFunc() +{ } -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_simplexSolver(simplexSolver), -m_pdSolver(pdSolver), -m_ownManifold (false), -m_manifoldPtr(mf), -m_lowLevelOfDetail(false), +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_pdSolver(pdSolver), + m_ownManifold(false), + m_manifoldPtr(mf), + m_lowLevelOfDetail(false), #ifdef USE_SEPDISTANCE_UTIL2 -m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), - (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), + m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), + (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), #endif -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) + m_numPerturbationIterations(numPerturbationIterations), + m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { (void)body0Wrap; (void)body1Wrap; } - - - btConvexConvexAlgorithm::~btConvexConvexAlgorithm() { if (m_ownManifold) @@ -221,339 +207,466 @@ btConvexConvexAlgorithm::~btConvexConvexAlgorithm() } } -void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) { m_lowLevelOfDetail = useLowLevel; } - struct btPerturbedContactResult : public btManifoldResult { btManifoldResult* m_originalManifoldResult; btTransform m_transformA; btTransform m_transformB; - btTransform m_unPerturbedTransform; - bool m_perturbA; - btIDebugDraw* m_debugDrawer; - - - btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer) - :m_originalManifoldResult(originalResult), - m_transformA(transformA), - m_transformB(transformB), - m_unPerturbedTransform(unPerturbedTransform), - m_perturbA(perturbA), - m_debugDrawer(debugDrawer) + btTransform m_unPerturbedTransform; + bool m_perturbA; + btIDebugDraw* m_debugDrawer; + + btPerturbedContactResult(btManifoldResult* originalResult, const btTransform& transformA, const btTransform& transformB, const btTransform& unPerturbedTransform, bool perturbA, btIDebugDraw* debugDrawer) + : m_originalManifoldResult(originalResult), + m_transformA(transformA), + m_transformB(transformB), + m_unPerturbedTransform(unPerturbedTransform), + m_perturbA(perturbA), + m_debugDrawer(debugDrawer) { } - virtual ~ btPerturbedContactResult() + virtual ~btPerturbedContactResult() { } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar orgDepth) { - btVector3 endPt,startPt; + btVector3 endPt, startPt; btScalar newDepth; btVector3 newNormal; if (m_perturbA) { - btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; - endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); - newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); - startPt = endPt+normalOnBInWorld*newDepth; - } else + btVector3 endPtOrg = pointInWorld + normalOnBInWorld * orgDepth; + endPt = (m_unPerturbedTransform * m_transformA.inverse())(endPtOrg); + newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); + startPt = endPt - normalOnBInWorld * newDepth; + } + else { - endPt = pointInWorld + normalOnBInWorld*orgDepth; - startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld); - newDepth = (endPt - startPt).dot(normalOnBInWorld); - + endPt = pointInWorld + normalOnBInWorld * orgDepth; + startPt = (m_unPerturbedTransform * m_transformB.inverse())(pointInWorld); + newDepth = (endPt - startPt).dot(normalOnBInWorld); } //#define DEBUG_CONTACTS 1 #ifdef DEBUG_CONTACTS - m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0)); - m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0)); - m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1)); -#endif //DEBUG_CONTACTS + m_debugDrawer->drawLine(startPt, endPt, btVector3(1, 0, 0)); + m_debugDrawer->drawSphere(startPt, 0.05, btVector3(0, 1, 0)); + m_debugDrawer->drawSphere(endPt, 0.05, btVector3(0, 0, 1)); +#endif //DEBUG_CONTACTS - - m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth); + m_originalManifoldResult->addContactPoint(normalOnBInWorld, startPt, newDepth); } - }; extern btScalar gContactBreakingThreshold; - // // Convex-Convex collision algorithm // -void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - if (!m_manifoldPtr) { //swapped? - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); - const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); - btVector3 normalOnB; - btVector3 pointOnBWorld; + btVector3 normalOnB; + btVector3 pointOnBWorld; #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { - btCapsuleShape* capsuleA = (btCapsuleShape*) min0; - btCapsuleShape* capsuleB = (btCapsuleShape*) min1; - // btVector3 localScalingA = capsuleA->getLocalScaling(); - // btVector3 localScalingB = capsuleB->getLocalScaling(); - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); - - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), - capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); - - if (dist<threshold) + //m_manifoldPtr->clearManifold(); + + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), + capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); + + if (dist < threshold) { - btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); - resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); } resultOut->refreshContactPoints(); return; } -#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE)) + { + //m_manifoldPtr->clearManifold(); + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btSphereShape* capsuleB = (btSphereShape*)min1; + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; -#ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); - } + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), + 0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1, + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); - if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) -#endif //USE_SEPDISTANCE_UTIL2 + if (dist < threshold) + { + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); + } + resultOut->refreshContactPoints(); + return; + } + if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { + //m_manifoldPtr->clearManifold(); + + btSphereShape* capsuleA = (btSphereShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; - - btGjkPairDetector::ClosestPointInput input; + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); - //TODO: if (dispatchInfo.m_useContinuous) - gjkPairDetector.setMinkowskiA(min0); - gjkPairDetector.setMinkowskiB(min1); + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(), + capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); + + if (dist < threshold) + { + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); + } + resultOut->refreshContactPoints(); + return; + } +#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - input.m_maximumDistanceSquared = BT_LARGE_FLOAT; - } else -#endif //USE_SEPDISTANCE_UTIL2 - { - //if (dispatchInfo.m_convexMaxDistanceUseCPT) - //{ - // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); - //} else - //{ - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); -// } - - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform()); } - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB = body1Wrap->getWorldTransform(); - - + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f) +#endif //USE_SEPDISTANCE_UTIL2 - + { + btGjkPairDetector::ClosestPointInput input; + btVoronoiSimplexSolver simplexSolver; + btGjkPairDetector gjkPairDetector(min0, min1, &simplexSolver, m_pdSolver); + //TODO: if (dispatchInfo.m_useContinuous) + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); #ifdef USE_SEPDISTANCE_UTIL2 - btScalar sepDist = 0.f; - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - sepDist = gjkPairDetector.getCachedSeparatingDistance(); - if (sepDist>SIMD_EPSILON) + if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; - //now perturbe directions to get multiple contact points - + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; + } + else +#endif //USE_SEPDISTANCE_UTIL2 + { + //if (dispatchInfo.m_convexMaxDistanceUseCPT) + //{ + // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + //} else + //{ + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold; + // } + + input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } - } -#endif //USE_SEPDISTANCE_UTIL2 - - if (min0->isPolyhedral() && min1->isPolyhedral()) - { + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB = body1Wrap->getWorldTransform(); - struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result +#ifdef USE_SEPDISTANCE_UTIL2 + btScalar sepDist = 0.f; + if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - virtual void setShapeIdentifiersA(int partId0,int index0){} - virtual void setShapeIdentifiersB(int partId1,int index1){} - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + sepDist = gjkPairDetector.getCachedSeparatingDistance(); + if (sepDist > SIMD_EPSILON) { + sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; + //now perturbe directions to get multiple contact points } - }; + } +#endif //USE_SEPDISTANCE_UTIL2 - - struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result + if (min0->isPolyhedral() && min1->isPolyhedral()) { - btDiscreteCollisionDetectorInterface::Result* m_originalResult; - btVector3 m_reportedNormalOnWorld; - btScalar m_marginOnA; - btScalar m_marginOnB; - btScalar m_reportedDistance; - - bool m_foundResult; - btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) - :m_originalResult(result), - m_marginOnA(marginOnA), - m_marginOnB(marginOnB), - m_foundResult(false) - { - } - - virtual void setShapeIdentifiersA(int partId0,int index0){} - virtual void setShapeIdentifiersB(int partId1,int index1){} - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) + struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result { - m_reportedDistance = depthOrg; - m_reportedNormalOnWorld = normalOnBInWorld; - - btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB; - m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB); - if (m_reportedDistance<0.f) + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_depth; + bool m_hasContact; + + btDummyResult() + : m_hasContact(false) { - m_foundResult = true; } - m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance); - } - }; - - btDummyResult dummy; - -///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it + virtual void setShapeIdentifiersA(int partId0, int index0) {} + virtual void setShapeIdentifiersB(int partId1, int index1) {} + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) + { + m_hasContact = true; + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + } + }; - btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin(); - btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin(); + struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result + { + btDiscreteCollisionDetectorInterface::Result* m_originalResult; + btVector3 m_reportedNormalOnWorld; + btScalar m_marginOnA; + btScalar m_marginOnB; + btScalar m_reportedDistance; + + bool m_foundResult; + btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) + : m_originalResult(result), + m_marginOnA(marginOnA), + m_marginOnB(marginOnB), + m_foundResult(false) + { + } - btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin); + virtual void setShapeIdentifiersA(int partId0, int index0) {} + virtual void setShapeIdentifiersB(int partId1, int index1) {} + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg) + { + m_reportedDistance = depthOrg; + m_reportedNormalOnWorld = normalOnBInWorld; - btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; - btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) - { + btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld * m_marginOnB; + m_reportedDistance = depthOrg + (m_marginOnA + m_marginOnB); + if (m_reportedDistance < 0.f) + { + m_foundResult = true; + } + m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance); + } + }; + btDummyResult dummy; - + ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btScalar min0Margin = min0->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min0->getMargin(); + btScalar min1Margin = min1->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min1->getMargin(); - btScalar minDist = -1e30f; - btVector3 sepNormalWorldSpace; - bool foundSepAxis = true; + btWithoutMarginResult withoutMargin(resultOut, min0Margin, min1Margin); - if (dispatchInfo.m_enableSatConvex) - { - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); - } else + btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*)min0; + btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1; + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) { + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; + + btScalar minDist = -1e30f; + btVector3 sepNormalWorldSpace; + bool foundSepAxis = true; + + if (dispatchInfo.m_enableSatConvex) + { + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace, *resultOut); + } + else + { #ifdef ZERO_MARGIN - gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); #else + gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw); + //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN + //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + //if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld; //gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = withoutMargin.m_reportedDistance; //gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); - gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw); - //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); -#endif //ZERO_MARGIN - //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - //if (l2>SIMD_EPSILON) - { - sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); - minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); - #ifdef ZERO_MARGIN - foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; + foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f; #else - foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin()); + foundSepAxis = withoutMargin.m_foundResult && minDist < 0; //-(min0->getMargin()+min1->getMargin()); #endif + } } - } - if (foundSepAxis) - { - -// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + if (foundSepAxis) + { + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); - + worldVertsB1.resize(0); + btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), minDist - threshold, threshold, worldVertsB1, worldVertsB2, + *resultOut); + } + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; } - if (m_ownManifold) + else { - resultOut->refreshContactPoints(); - } - return; + //we can also deal with convex versus triangle (without connectivity data) + if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) + { + btVertexArray worldSpaceVertices; + btTriangleShape* tri = (btTriangleShape*)polyhedronB; + worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[0]); + worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[1]); + worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[2]); - } else - { - //we can also deal with convex versus triangle (without connectivity data) - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) - { + //tri->initializePolyhedralFeatures(); - btVertexArray vertices; - btTriangleShape* tri = (btTriangleShape*)polyhedronB; - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); - - //tri->initializePolyhedralFeatures(); + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btVector3 sepNormalWorldSpace; + btScalar minDist = -1e30f; + btScalar maxDist = threshold; - btVector3 sepNormalWorldSpace; - btScalar minDist =-1e30f; - btScalar maxDist = threshold; - - bool foundSepAxis = false; - if (0) - { - polyhedronB->initializePolyhedralFeatures(); - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); - // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - - } else - { + bool foundSepAxis = false; + bool useSatSepNormal = true; + + if (useSatSepNormal) + { +#if 0 + if (0) + { + //initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle + polyhedronB->initializePolyhedralFeatures(); + } else +#endif + { + btVector3 uniqueEdges[3] = {tri->m_vertices1[1] - tri->m_vertices1[0], + tri->m_vertices1[2] - tri->m_vertices1[1], + tri->m_vertices1[0] - tri->m_vertices1[2]}; + + uniqueEdges[0].normalize(); + uniqueEdges[1].normalize(); + uniqueEdges[2].normalize(); + + btConvexPolyhedron polyhedron; + polyhedron.m_vertices.push_back(tri->m_vertices1[2]); + polyhedron.m_vertices.push_back(tri->m_vertices1[0]); + polyhedron.m_vertices.push_back(tri->m_vertices1[1]); + + { + btFace combinedFaceA; + combinedFaceA.m_indices.push_back(0); + combinedFaceA.m_indices.push_back(1); + combinedFaceA.m_indices.push_back(2); + btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq = 1e30f; + for (int v = 0; v < combinedFaceA.m_indices.size(); v++) + { + btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal); + if (planeEq > eq) + { + planeEq = eq; + } + } + combinedFaceA.m_plane[0] = faceNormal[0]; + combinedFaceA.m_plane[1] = faceNormal[1]; + combinedFaceA.m_plane[2] = faceNormal[2]; + combinedFaceA.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceA); + } + { + btFace combinedFaceB; + combinedFaceB.m_indices.push_back(0); + combinedFaceB.m_indices.push_back(2); + combinedFaceB.m_indices.push_back(1); + btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq = 1e30f; + for (int v = 0; v < combinedFaceB.m_indices.size(); v++) + { + btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal); + if (planeEq > eq) + { + planeEq = eq; + } + } + + combinedFaceB.m_plane[0] = faceNormal[0]; + combinedFaceB.m_plane[1] = faceNormal[1]; + combinedFaceB.m_plane[2] = faceNormal[2]; + combinedFaceB.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceB); + } + + polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]); + polyhedron.initialize2(); + + polyhedronB->setPolyhedralFeatures(polyhedron); + } + + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace, *resultOut); + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + } + else + { #ifdef ZERO_MARGIN - gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); #else - gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); -#endif//ZERO_MARGIN - + gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN + + if (dummy.m_hasContact && dummy.m_depth < 0) + { + if (foundSepAxis) + { + if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace) < 0.99) + { + printf("?\n"); + } + } + else + { + printf("!\n"); + } + sepNormalWorldSpace.setValue(0, 0, 1); // = dummy.m_normalOnBInWorld; + //minDist = dummy.m_depth; + foundSepAxis = true; + } +#if 0 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2>SIMD_EPSILON) { @@ -563,143 +676,132 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); foundSepAxis = true; } - } +#endif + } - - if (foundSepAxis) - { - btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); - } - - - if (m_ownManifold) - { - resultOut->refreshContactPoints(); + if (foundSepAxis) + { + worldVertsB2.resize(0); + btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2, minDist - threshold, maxDist, *resultOut); + } + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + + return; } - - return; } - } + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); - } - - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - - //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points - if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) - { - - int i; - btVector3 v0,v1; - btVector3 sepNormalWorldSpace; - btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - - if (l2>SIMD_EPSILON) + //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points + if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - - btPlaneSpace1(sepNormalWorldSpace,v0,v1); - + int i; + btVector3 v0, v1; + btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - bool perturbeA = true; - const btScalar angleLimit = 0.125f * SIMD_PI; - btScalar perturbeAngle; - btScalar radiusA = min0->getAngularMotionDisc(); - btScalar radiusB = min1->getAngularMotionDisc(); - if (radiusA < radiusB) - { - perturbeAngle = gContactBreakingThreshold /radiusA; - perturbeA = true; - } else + if (l2 > SIMD_EPSILON) { - perturbeAngle = gContactBreakingThreshold / radiusB; - perturbeA = false; - } - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis() * (1.f / l2); - btTransform unPerturbedTransform; - if (perturbeA) - { - unPerturbedTransform = input.m_transformA; - } else - { - unPerturbedTransform = input.m_transformB; - } - - for ( i=0;i<m_numPerturbationIterations;i++) - { - if (v0.length2()>SIMD_EPSILON) + btPlaneSpace1(sepNormalWorldSpace, v0, v1); + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) + { + perturbeAngle = gContactBreakingThreshold / radiusA; + perturbeA = true; + } + else { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); - - + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if (perturbeAngle > angleLimit) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; if (perturbeA) { - input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis()); - input.m_transformB = body1Wrap->getWorldTransform(); - #ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); - #endif //DEBUG_CONTACTS - } else + unPerturbedTransform = input.m_transformA; + } + else { - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis()); - #ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); - #endif + unPerturbedTransform = input.m_transformB; } - - btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); - gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + + for (i = 0; i < m_numPerturbationIterations; i++) + { + if (v0.length2() > SIMD_EPSILON) + { + btQuaternion perturbeRot(v0, perturbeAngle); + btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations)); + btQuaternion rotq(sepNormalWorldSpace, iterationAngle); + + if (perturbeA) + { + input.m_transformA.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0Wrap->getWorldTransform().getBasis()); + input.m_transformB = body1Wrap->getWorldTransform(); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 10.0); +#endif //DEBUG_CONTACTS + } + else + { + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1Wrap->getWorldTransform().getBasis()); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB, 10.0); +#endif + } + + btPerturbedContactResult perturbedResultOut(resultOut, input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input, perturbedResultOut, dispatchInfo.m_debugDraw); + } } } } - } - - #ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) - { - m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); - } -#endif //USE_SEPDISTANCE_UTIL2 - - + if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > SIMD_EPSILON)) + { + m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform()); + } +#endif //USE_SEPDISTANCE_UTIL2 } if (m_ownManifold) { resultOut->refreshContactPoints(); } - } - - bool disableCcd = false; -btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold - + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, btScalar resultFraction = btScalar(1.); - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); - + if (squareMot0 < col0->getCcdSquareMotionThreshold() && squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; @@ -707,77 +809,65 @@ btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, if (disableCcd) return btScalar(1.); - //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - /// Convex0 against sphere for Convex1 { btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } - - - - } /// Sphere (for convex0) against Convex1 { btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape()); - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } } - - return resultFraction; + return resultFraction; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index 51db0c6548d..eac5b4d8248 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -23,7 +23,8 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" -#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" class btConvexPenetrationDepthSolver; @@ -40,69 +41,61 @@ class btConvexPenetrationDepthSolver; class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { #ifdef USE_SEPDISTANCE_UTIL2 - btConvexSeparatingDistanceUtil m_sepDistance; + btConvexSeparatingDistanceUtil m_sepDistance; #endif - btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_pdSolver; - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - + btVertexArray worldVertsB1; + btVertexArray worldVertsB2; + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - ///cache separating vector to speedup collision detection - public: - - btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvexConvexAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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) + 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); - void setLowLevelOfDetail(bool useLowLevel); - - - const btPersistentManifold* getManifold() + const btPersistentManifold* getManifold() { return m_manifoldPtr; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - + CreateFunc(btConvexPenetrationDepthSolver* pdSolver); + virtual ~CreateFunc(); - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); - return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvexConvexAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } }; - - }; -#endif //BT_CONVEX_CONVEX_ALGORITHM_H +#endif //BT_CONVEX_CONVEX_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index cce2d95bcf9..ba1bc06b69a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -23,25 +23,24 @@ subject to the following restrictions: //#include <stdio.h> -btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) -: btCollisionAlgorithm(ci), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped), -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold) + : btCollisionAlgorithm(ci), + m_ownManifold(false), + m_manifoldPtr(mf), + m_isSwapped(isSwapped), + m_numPerturbationIterations(numPerturbationIterations), + m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? col0Wrap : col1Wrap; - if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject()); m_ownManifold = true; } } - btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() { if (m_ownManifold) @@ -51,32 +50,32 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() } } -void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape(); - bool hasCollision = false; + bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - + btTransform convexWorldTransform = convexObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; + convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; //now perturbe the convex-world transform - convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); + convexWorldTransform.getBasis() *= btMatrix3x3(perturbeRot); btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform(); - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + planeInConvex = convexWorldTransform.inverse() * planeObjWrap->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); @@ -86,70 +85,69 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance); } } - -void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape(); bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); btTransform planeInConvex; - planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); + planeInConvex = convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); + convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; - hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; resultOut->setPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance); } //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: //they keep on rolling forever because of the additional off-center contact points //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) - if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) + if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { - btVector3 v0,v1; - btPlaneSpace1(planeNormal,v0,v1); + btVector3 v0, v1; + btPlaneSpace1(planeNormal, v0, v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radius = convexShape->getAngularMotionDisc(); perturbeAngle = gContactBreakingThreshold / radius; - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; + if (perturbeAngle > angleLimit) + perturbeAngle = angleLimit; - btQuaternion perturbeRot(v0,perturbeAngle); - for (int i=0;i<m_numPerturbationIterations;i++) + btQuaternion perturbeRot(v0, perturbeAngle); + for (int i = 0; i < m_numPerturbationIterations; i++) { - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(planeNormal,iterationAngle); - collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut); + btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations)); + btQuaternion rotq(planeNormal, iterationAngle); + collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0Wrap, body1Wrap, dispatchInfo, resultOut); } } @@ -162,7 +160,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectW } } -btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index d28c430c4c1..b693da118fe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -28,25 +28,24 @@ class btPersistentManifold; /// Other features are frame-coherency (persistent data) and collision response. class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; public: - - btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvexPlaneCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - void collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -54,31 +53,30 @@ public: } } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - int m_numPerturbationIterations; + int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - - CreateFunc() + + CreateFunc() : m_numPerturbationIterations(1), - m_minimumPointsPerturbationThreshold(0) + m_minimumPointsPerturbationThreshold(0) { } - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); - } else + return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); + } + else { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } } }; - }; -#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H - +#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index d42f00a637f..ef3ea9e394c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -26,115 +26,108 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#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/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(); - + void* mem = NULL; if (constructionInfo.m_useEpaPenetrationAlgorithm) { - mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); - m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; - }else + mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver), 16); + m_pdSolver = new (mem) btGjkEpaPenetrationDepthSolver; + } + else { - mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); - m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; + mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver), 16); + m_pdSolver = new (mem) btMinkowskiPenetrationDepthSolver; } - + //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(btCompoundCompoundCollisionAlgorithm::CreateFunc),16); - m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); - m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; - mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); - m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); - m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc), 16); + m_convexConvexCreateFunc = new (mem) btConvexConvexAlgorithm::CreateFunc(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(btCompoundCompoundCollisionAlgorithm::CreateFunc), 16); + m_compoundCompoundCreateFunc = new (mem) btCompoundCompoundCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc), 16); + m_swappedCompoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc), 16); + 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; + 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 +#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; + 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; + + mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc), 16); + m_boxBoxCF = new (mem) btBoxBoxCollisionAlgorithm::CreateFunc; //convex versus plane - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16); m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + 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(btCompoundCompoundCollisionAlgorithm); - int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); - + int collisionAlgorithmMaxElementSize = btMax(maxSize, constructionInfo.m_customCollisionAlgorithmMaxElementSize); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize2); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize3); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize4); + if (constructionInfo.m_persistentManifoldPool) { m_ownsPersistentManifoldPool = false; m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; - } else + } + else { m_ownsPersistentManifoldPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold), constructionInfo.m_defaultMaxPersistentManifoldPoolSize); } - + + collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize + 16) & 0xffffffffffff0; if (constructionInfo.m_collisionAlgorithmPool) { m_ownsCollisionAlgorithmPool = false; m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; - } else + } + else { m_ownsCollisionAlgorithmPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } - - } btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() @@ -151,95 +144,159 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() } m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConvexCreateFunc); + btAlignedFree(m_convexConvexCreateFunc); m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConcaveCreateFunc); + btAlignedFree(m_convexConcaveCreateFunc); m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedConvexConcaveCreateFunc); + btAlignedFree(m_swappedConvexConcaveCreateFunc); m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_compoundCreateFunc); + btAlignedFree(m_compoundCreateFunc); m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); btAlignedFree(m_compoundCompoundCreateFunc); m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedCompoundCreateFunc); + btAlignedFree(m_swappedCompoundCreateFunc); m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_emptyCreateFunc); + btAlignedFree(m_emptyCreateFunc); m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereSphereCF); + btAlignedFree(m_sphereSphereCF); #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereBoxCF); + btAlignedFree(m_sphereBoxCF); m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxSphereCF); -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + btAlignedFree(m_boxSphereCF); +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereTriangleCF); + btAlignedFree(m_sphereTriangleCF); m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_triangleSphereCF); + btAlignedFree(m_triangleSphereCF); m_boxBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxBoxCF); + btAlignedFree(m_boxBoxCF); m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexPlaneCF); + btAlignedFree(m_convexPlaneCF); m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_planeConvexCF); - - m_simplexSolver->~btVoronoiSimplexSolver(); - btAlignedFree(m_simplexSolver); + btAlignedFree(m_planeConvexCF); m_pdSolver->~btConvexPenetrationDepthSolver(); - + btAlignedFree(m_pdSolver); +} +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(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; + } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) -{ + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + + 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 ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) { - return m_sphereSphereCF; + return m_swappedConvexConcaveCreateFunc; } -#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + + if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { - return m_sphereBoxCF; + return m_compoundCompoundCreateFunc; } - if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if (btBroadphaseProxy::isCompound(proxyType0)) { - return m_boxSphereCF; + return m_compoundCreateFunc; + } + else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } } -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + //failed to find an algorithm + return m_emptyCreateFunc; +} - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) +{ + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_sphereTriangleCF; + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; } - if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_triangleSphereCF; - } + 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; @@ -249,8 +306,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg { return m_planeConvexCF; } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { @@ -267,7 +322,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_swappedConvexConcaveCreateFunc; } - if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { return m_compoundCompoundCreateFunc; @@ -276,7 +330,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg if (btBroadphaseProxy::isCompound(proxyType0)) { return m_compoundCreateFunc; - } else + } + else { if (btBroadphaseProxy::isCompound(proxyType1)) { @@ -290,17 +345,17 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) { - btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc; + btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*)m_convexConvexCreateFunc; convexConvex->m_numPerturbationIterations = numPerturbationIterations; convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; } -void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) { btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF; cpCF->m_numPerturbationIterations = numPerturbationIterations; cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; - + btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF; pcCF->m_numPerturbationIterations = numPerturbationIterations; pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 2078420e198..b39a3f41de1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -20,77 +20,68 @@ subject to the following restrictions: class btVoronoiSimplexSolver; class btConvexPenetrationDepthSolver; -struct btDefaultCollisionConstructionInfo +struct btDefaultCollisionConstructionInfo { - btPoolAllocator* m_persistentManifoldPool; - btPoolAllocator* m_collisionAlgorithmPool; - int m_defaultMaxPersistentManifoldPoolSize; - int m_defaultMaxCollisionAlgorithmPoolSize; - int m_customCollisionAlgorithmMaxElementSize; - int m_useEpaPenetrationAlgorithm; + btPoolAllocator* m_persistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + int m_defaultMaxPersistentManifoldPoolSize; + int m_defaultMaxCollisionAlgorithmPoolSize; + int m_customCollisionAlgorithmMaxElementSize; + int m_useEpaPenetrationAlgorithm; btDefaultCollisionConstructionInfo() - :m_persistentManifoldPool(0), - m_collisionAlgorithmPool(0), - m_defaultMaxPersistentManifoldPoolSize(4096), - m_defaultMaxCollisionAlgorithmPoolSize(4096), - m_customCollisionAlgorithmMaxElementSize(0), - m_useEpaPenetrationAlgorithm(true) + : m_persistentManifoldPool(0), + m_collisionAlgorithmPool(0), + m_defaultMaxPersistentManifoldPoolSize(4096), + m_defaultMaxCollisionAlgorithmPoolSize(4096), + m_customCollisionAlgorithmMaxElementSize(0), + m_useEpaPenetrationAlgorithm(true) { } }; - - ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator, pool memory allocators ///@todo: describe the meaning -class btDefaultCollisionConfiguration : public btCollisionConfiguration +class btDefaultCollisionConfiguration : public btCollisionConfiguration { - protected: + int m_persistentManifoldPoolSize; - int m_persistentManifoldPoolSize; - - - btPoolAllocator* m_persistentManifoldPool; - bool m_ownsPersistentManifoldPool; + btPoolAllocator* m_persistentManifoldPool; + bool m_ownsPersistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + bool m_ownsCollisionAlgorithmPool; - btPoolAllocator* m_collisionAlgorithmPool; - bool m_ownsCollisionAlgorithmPool; + //default penetration depth solver + btConvexPenetrationDepthSolver* m_pdSolver; - //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_compoundCompoundCreateFunc; - - btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; + + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; btCollisionAlgorithmCreateFunc* m_sphereSphereCF; btCollisionAlgorithmCreateFunc* m_sphereBoxCF; btCollisionAlgorithmCreateFunc* m_boxSphereCF; btCollisionAlgorithmCreateFunc* m_boxBoxCF; - btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; - btCollisionAlgorithmCreateFunc* m_triangleSphereCF; - btCollisionAlgorithmCreateFunc* m_planeConvexCF; - btCollisionAlgorithmCreateFunc* m_convexPlaneCF; - -public: - + btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; + btCollisionAlgorithmCreateFunc* m_triangleSphereCF; + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; +public: btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); virtual ~btDefaultCollisionConfiguration(); - ///memory pools + ///memory pools virtual btPoolAllocator* getPersistentManifoldPool() { return m_persistentManifoldPool; @@ -101,14 +92,9 @@ public: return m_collisionAlgorithmPool; } + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1); - virtual btVoronoiSimplexSolver* getSimplexSolver() - { - return m_simplexSolver; - } - - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1); ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. ///By default, this feature is disabled for best performance. @@ -117,11 +103,9 @@ public: ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first. ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points. ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. - void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); - - void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + void setConvexConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3); + void setPlaneConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3); }; -#endif //BT_DEFAULT_COLLISION_CONFIGURATION - +#endif //BT_DEFAULT_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp index 5fa1c8be5e4..7cd41bdb33e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -15,20 +15,16 @@ subject to the following restrictions: #include "btEmptyCollisionAlgorithm.h" - - btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btCollisionAlgorithm(ci) { } -void btEmptyAlgorithm::processCollision (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* ) +void btEmptyAlgorithm::processCollision(const btCollisionObjectWrapper*, const btCollisionObjectWrapper*, const btDispatcherInfo&, btManifoldResult*) { } -btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) +btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject*, btCollisionObject*, const btDispatcherInfo&, btManifoldResult*) { return btScalar(1.); } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index cb0f152183c..65ef83e0946 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -25,30 +25,28 @@ subject to the following restrictions: ///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. class btEmptyAlgorithm : public btCollisionAlgorithm { - public: - btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + { (void)body0Wrap; (void)body1Wrap; void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm)); - return new(mem) btEmptyAlgorithm(ci); + return new (mem) btEmptyAlgorithm(ci); } }; } ATTRIBUTE_ALIGNED(16); -#endif //BT_EMPTY_ALGORITH +#endif //BT_EMPTY_ALGORITH diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp index 86141fa6899..00f16fd0a8a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -29,60 +29,58 @@ btGhostObject::~btGhostObject() btAssert(!m_overlappingObjects.size()); } - -void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) + if (index == m_overlappingObjects.size()) { //not found m_overlappingObjects.push_back(otherObject); } } -void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) +void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index<m_overlappingObjects.size()) + if (index < m_overlappingObjects.size()) { - m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1]; m_overlappingObjects.pop_back(); } } - btPairCachingGhostObject::btPairCachingGhostObject() { - m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); + m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16)) btHashedOverlappingPairCache(); } btPairCachingGhostObject::~btPairCachingGhostObject() { m_hashPairCache->~btHashedOverlappingPairCache(); - btAlignedFree( m_hashPairCache ); + btAlignedFree(m_hashPairCache); } -void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy) { - btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); + btBroadphaseProxy* actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); btAssert(actualThisProxy); btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) + if (index == m_overlappingObjects.size()) { m_overlappingObjects.push_back(otherObject); - m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + m_hashPairCache->addOverlappingPair(actualThisProxy, otherProxy); } } -void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy1) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); @@ -90,82 +88,79 @@ void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index<m_overlappingObjects.size()) + if (index < m_overlappingObjects.size()) { - m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1]; m_overlappingObjects.pop_back(); - m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher); + m_hashPairCache->removeOverlappingPair(actualThisProxy, otherProxy, dispatcher); } } - -void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { - btTransform convexFromTrans,convexToTrans; + 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); + 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); + 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_overlappingObjects.size();i++) + for (i = 0; i < m_overlappingObjects.size(); i++) { - btCollisionObject* collisionObject= m_overlappingObjects[i]; + btCollisionObject* collisionObject = m_overlappingObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + 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 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)) + if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) { - btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + btCollisionWorld::objectQuerySingle(castShape, convexFromTrans, convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } - } -void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const +void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const { btTransform rayFromTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFromWorld); - btTransform rayToTrans; + btTransform rayToTrans; rayToTrans.setIdentity(); rayToTrans.setOrigin(rayToWorld); - int i; - for (i=0;i<m_overlappingObjects.size();i++) + for (i = 0; i < m_overlappingObjects.size(); i++) { - btCollisionObject* collisionObject= m_overlappingObjects[i]; + btCollisionObject* collisionObject = m_overlappingObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback); + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); } } } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h index 8ec86138575..aa7f48d5cb4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_GHOST_OBJECT_H #define BT_GHOST_OBJECT_H - #include "btCollisionObject.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" #include "LinearMath/btAlignedAllocator.h" @@ -31,48 +30,47 @@ class btDispatcher; ///By default, this overlap is based on the AABB ///This is useful for creating a character controller, collision sensors/triggers, explosions etc. ///We plan on adding rayTest and other queries for the btGhostObject -ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject +ATTRIBUTE_ALIGNED16(class) +btGhostObject : public btCollisionObject { protected: - btAlignedObjectArray<btCollisionObject*> m_overlappingObjects; public: - btGhostObject(); virtual ~btGhostObject(); - void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; + void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + virtual void addOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btBroadphaseProxy* thisProxy = 0); ///this method is mainly for expert/internal use only. - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + virtual void removeOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btDispatcher * dispatcher, btBroadphaseProxy* thisProxy = 0); - int getNumOverlappingObjects() const + int getNumOverlappingObjects() const { return m_overlappingObjects.size(); } - btCollisionObject* getOverlappingObject(int index) + btCollisionObject* getOverlappingObject(int index) { return m_overlappingObjects[index]; } - const btCollisionObject* getOverlappingObject(int index) const + const btCollisionObject* getOverlappingObject(int index) const { return m_overlappingObjects[index]; } - btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs() + btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs() { return m_overlappingObjects; } - const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const + const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const { return m_overlappingObjects; } @@ -81,49 +79,43 @@ public: // internal cast // - static const btGhostObject* upcast(const btCollisionObject* colObj) + static const btGhostObject* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()==CO_GHOST_OBJECT) + if (colObj->getInternalType() == CO_GHOST_OBJECT) return (const btGhostObject*)colObj; return 0; } - static btGhostObject* upcast(btCollisionObject* colObj) + static btGhostObject* upcast(btCollisionObject * colObj) { - if (colObj->getInternalType()==CO_GHOST_OBJECT) + if (colObj->getInternalType() == CO_GHOST_OBJECT) return (btGhostObject*)colObj; return 0; } - }; -class btPairCachingGhostObject : public btGhostObject +class btPairCachingGhostObject : public btGhostObject { - btHashedOverlappingPairCache* m_hashPairCache; + btHashedOverlappingPairCache* m_hashPairCache; public: - btPairCachingGhostObject(); virtual ~btPairCachingGhostObject(); ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy = 0); - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy = 0); - btHashedOverlappingPairCache* getOverlappingPairCache() + btHashedOverlappingPairCache* getOverlappingPairCache() { return m_hashPairCache; } - }; - - ///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. class btGhostPairCallback : public btOverlappingPairCallback { - public: btGhostPairCallback() { @@ -131,15 +123,14 @@ public: virtual ~btGhostPairCallback() { - } - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); if (ghost0) ghost0->addOverlappingObjectInternal(proxy1, proxy0); if (ghost1) @@ -147,29 +138,25 @@ public: return 0; } - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); if (ghost0) - ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); + ghost0->removeOverlappingObjectInternal(proxy1, dispatcher, proxy0); if (ghost1) - ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); + ghost1->removeOverlappingObjectInternal(proxy0, dispatcher, proxy1); return 0; } - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/) { btAssert(0); //need to keep track of all ghost objects and call them here //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); } - - - }; #endif - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 8c8a7c3c1e3..b686d98d1e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -13,61 +13,49 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btHashedSimplePairCache.h" - #include <stdio.h> -int gOverlappingSimplePairs = 0; -int gRemoveSimplePairs =0; -int gAddedSimplePairs =0; -int gFindSimplePairs =0; - - - +#ifdef BT_DEBUG_COLLISION_PAIRS +int gOverlappingSimplePairs = 0; +int gRemoveSimplePairs = 0; +int gAddedSimplePairs = 0; +int gFindSimplePairs = 0; +#endif //BT_DEBUG_COLLISION_PAIRS -btHashedSimplePairCache::btHashedSimplePairCache() { - int initialAllocatedSize= 2; +btHashedSimplePairCache::btHashedSimplePairCache() +{ + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - - btHashedSimplePairCache::~btHashedSimplePairCache() { } - - - - - void btHashedSimplePairCache::removeAllPairs() { m_overlappingPairArray.clear(); m_hashTable.clear(); m_next.clear(); - int initialAllocatedSize= 2; + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gFindSimplePairs++; - - +#endif + /*if (indexA > indexB) btSwap(indexA, indexB);*/ - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); if (hash >= m_hashTable.size()) { @@ -92,9 +80,8 @@ btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) //#include <stdio.h> -void btHashedSimplePairCache::growTables() +void btHashedSimplePairCache::growTables() { - int newCapacity = m_overlappingPairArray.capacity(); if (m_hashTable.size() < newCapacity) @@ -105,10 +92,9 @@ void btHashedSimplePairCache::growTables() m_hashTable.resize(newCapacity); m_next.resize(newCapacity); - int i; - for (i= 0; i < newCapacity; ++i) + for (i = 0; i < newCapacity; ++i) { m_hashTable[i] = BT_SIMPLE_NULL_PAIR; } @@ -117,27 +103,22 @@ void btHashedSimplePairCache::growTables() m_next[i] = BT_SIMPLE_NULL_PAIR; } - for(i=0;i<curHashtableSize;i++) + for (i = 0; i < curHashtableSize; i++) { - const btSimplePair& pair = m_overlappingPairArray[i]; int indexA = pair.m_indexA; int indexB = pair.m_indexB; - - int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask m_next[i] = m_hashTable[hashValue]; m_hashTable[hashValue] = i; } - - } } btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) { - - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask btSimplePair* pair = internalFindPair(indexA, indexB, hash); if (pair != NULL) @@ -155,30 +136,29 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) { growTables(); //hash with new capacity - hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); } - - pair = new (mem) btSimplePair(indexA,indexB); + + pair = new (mem) btSimplePair(indexA, indexB); pair->m_userPointer = 0; - + m_next[count] = m_hashTable[hash]; m_hashTable[hash] = count; return pair; } - - void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gRemoveSimplePairs++; - +#endif /*if (indexA > indexB) btSwap(indexA, indexB);*/ - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); btSimplePair* pair = internalFindPair(indexA, indexB, hash); if (pair == NULL) @@ -186,10 +166,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) return 0; } - void* userData = pair->m_userPointer; - int pairIndex = int(pair - &m_overlappingPairArray[0]); btAssert(pairIndex < m_overlappingPairArray.size()); @@ -229,8 +207,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) // Remove the last pair from the hash table. const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; - /* missing swap here too, Nat. */ - int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); + /* missing swap here too, Nat. */ + int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity() - 1)); index = m_hashTable[lastHash]; btAssert(index != BT_SIMPLE_NULL_PAIR); @@ -264,13 +242,3 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) return userData; } //#include <stdio.h> - - - - - - - - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 186964d723e..fd38a4f0e19 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -16,142 +16,126 @@ subject to the following restrictions: #ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H #define BT_HASHED_SIMPLE_PAIR_CACHE_H - - #include "LinearMath/btAlignedObjectArray.h" -const int BT_SIMPLE_NULL_PAIR=0xffffffff; +const int BT_SIMPLE_NULL_PAIR = 0xffffffff; struct btSimplePair { - btSimplePair(int indexA,int indexB) - :m_indexA(indexA), - m_indexB(indexB), - m_userPointer(0) + btSimplePair(int indexA, int indexB) + : m_indexA(indexA), + m_indexB(indexB), + m_userPointer(0) { } int m_indexA; int m_indexB; - union - { - void* m_userPointer; - int m_userValue; + union { + void* m_userPointer; + int m_userValue; }; }; -typedef btAlignedObjectArray<btSimplePair> btSimplePairArray; - - +typedef btAlignedObjectArray<btSimplePair> btSimplePairArray; +#ifdef BT_DEBUG_COLLISION_PAIRS extern int gOverlappingSimplePairs; extern int gRemoveSimplePairs; extern int gAddedSimplePairs; extern int gFindSimplePairs; - - - +#endif //BT_DEBUG_COLLISION_PAIRS class btHashedSimplePairCache { - btSimplePairArray m_overlappingPairArray; - + btSimplePairArray m_overlappingPairArray; protected: - - btAlignedObjectArray<int> m_hashTable; - btAlignedObjectArray<int> m_next; - + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; public: btHashedSimplePairCache(); virtual ~btHashedSimplePairCache(); - + void removeAllPairs(); - virtual void* removeOverlappingPair(int indexA,int indexB); - + virtual void* removeOverlappingPair(int indexA, int indexB); + // Add a pair and return the new pair. If the pair already exists, // no new pair is created and the old one is returned. - virtual btSimplePair* addOverlappingPair(int indexA,int indexB) + virtual btSimplePair* addOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gAddedSimplePairs++; +#endif - return internalAddPair(indexA,indexB); + return internalAddPair(indexA, indexB); } - - virtual btSimplePair* getOverlappingPairArrayPtr() + virtual btSimplePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; } - const btSimplePair* getOverlappingPairArrayPtr() const + const btSimplePair* getOverlappingPairArrayPtr() const { return &m_overlappingPairArray[0]; } - btSimplePairArray& getOverlappingPairArray() + btSimplePairArray& getOverlappingPairArray() { return m_overlappingPairArray; } - const btSimplePairArray& getOverlappingPairArray() const + const btSimplePairArray& getOverlappingPairArray() const { return m_overlappingPairArray; } - - btSimplePair* findPair(int indexA,int indexB); + btSimplePair* findPair(int indexA, int indexB); int GetCount() const { return m_overlappingPairArray.size(); } - int getNumOverlappingPairs() const + int getNumOverlappingPairs() const { return m_overlappingPairArray.size(); } + private: - - btSimplePair* internalAddPair(int indexA, int indexB); + btSimplePair* internalAddPair(int indexA, int indexB); - void growTables(); + void growTables(); SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) - { + { return pair.m_indexA == indexA && pair.m_indexB == indexB; } - - - SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) { - int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); + unsigned int key = indexA | (indexB << 16); // Thomas Wang's hash key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); key += ~(key << 11); - key ^= (key >> 16); - return static_cast<unsigned int>(key); + key ^= (key >> 16); + return key; } - - - - - SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) + SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA, int proxyIdB, int hash) { - int index = m_hashTable[hash]; - - while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) + + while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) { index = m_next[index]; } - if ( index == BT_SIMPLE_NULL_PAIR ) + if (index == BT_SIMPLE_NULL_PAIR) { return NULL; } @@ -160,13 +144,6 @@ private: return &m_overlappingPairArray[index]; } - - }; - - - -#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H - - +#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 6cba442ca5d..a71700f58af 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -1,6 +1,8 @@ #include "btInternalEdgeUtility.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" + #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" @@ -12,50 +14,44 @@ #ifdef DEBUG_INTERNAL_EDGE #include <stdio.h> -#endif //DEBUG_INTERNAL_EDGE - +#endif //DEBUG_INTERNAL_EDGE #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW static btIDebugDraw* gDebugDrawer = 0; -void btSetDebugDrawer(btIDebugDraw* debugDrawer) +void btSetDebugDrawer(btIDebugDraw* debugDrawer) { gDebugDrawer = debugDrawer; } -static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color) +static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color) { if (gDebugDrawer) - gDebugDrawer->drawLine(from,to,color); + gDebugDrawer->drawLine(from, to, color); } -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW -static int btGetHash(int partId, int triangleIndex) +static int btGetHash(int partId, int triangleIndex) { - int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + int hash = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; return hash; } - - -static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB) +static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB) { - const btVector3 refAxis0 = edgeA; - const btVector3 refAxis1 = normalA; + const btVector3 refAxis0 = edgeA; + const btVector3 refAxis1 = normalA; const btVector3 swingAxis = normalB; btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); - return angle; + return angle; } - struct btConnectivityProcessor : public btTriangleCallback { - int m_partIdA; - int m_triangleIndexA; - btVector3* m_triangleVerticesA; - btTriangleInfoMap* m_triangleInfoMap; - + int m_partIdA; + int m_triangleIndexA; + btVector3* m_triangleVerticesA; + btTriangleInfoMap* m_triangleInfoMap; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { @@ -69,18 +65,17 @@ struct btConnectivityProcessor : public btTriangleCallback //search for shared vertices and edges int numshared = 0; - int sharedVertsA[3]={-1,-1,-1}; - int sharedVertsB[3]={-1,-1,-1}; + int sharedVertsA[3] = {-1, -1, -1}; + int sharedVertsB[3] = {-1, -1, -1}; ///skip degenerate triangles - btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2(); + btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2(); if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) return; - - btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2(); + btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2(); ///skip degenerate triangles - if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold) + if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold) return; #if 0 @@ -96,36 +91,36 @@ struct btConnectivityProcessor : public btTriangleCallback triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); #endif - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - for (int j=0;j<3;j++) + for (int j = 0; j < 3; j++) { - if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) + if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } } ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } switch (numshared) { - case 0: + case 0: { break; } - case 1: + case 1: { //shared vertex break; } - case 2: + case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct @@ -138,26 +133,25 @@ struct btConnectivityProcessor : public btTriangleCallback sharedVertsB[0] = tmp; } - int hash = btGetHash(m_partIdA,m_triangleIndexA); + int hash = btGetHash(m_partIdA, m_triangleIndexA); btTriangleInfo* info = m_triangleInfoMap->find(hash); if (!info) { btTriangleInfo tmp; - m_triangleInfoMap->insert(hash,tmp); + m_triangleInfoMap->insert(hash, tmp); info = m_triangleInfoMap->find(hash); } - int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; - int otherIndexA = 3-sumvertsA; + int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; + int otherIndexA = 3 - sumvertsA; - - btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]); + btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]); - btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]); - int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]); + btTriangleShape tA(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]); + int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]); - btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); + btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); btVector3 normalA; @@ -168,26 +162,25 @@ struct btConnectivityProcessor : public btTriangleCallback btVector3 edgeCrossA = edge.cross(normalA).normalize(); { - btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; + btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (edgeCrossA.dot(tmp) < 0) { - edgeCrossA*=-1; + edgeCrossA *= -1; } } btVector3 edgeCrossB = edge.cross(normalB).normalize(); { - btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; + btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (edgeCrossB.dot(tmp) < 0) { - edgeCrossB*=-1; + edgeCrossB *= -1; } } - btScalar angle2 = 0; - btScalar ang4 = 0.f; - + btScalar angle2 = 0; + btScalar ang4 = 0.f; btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); btScalar len2 = calculatedEdge.length2(); @@ -196,52 +189,47 @@ struct btConnectivityProcessor : public btTriangleCallback //btVector3 calculatedNormalB = normalA; bool isConvex = false; - if (len2<m_triangleInfoMap->m_planarEpsilon) + if (len2 < m_triangleInfoMap->m_planarEpsilon) { angle2 = 0.f; ang4 = 0.f; - } else + } + else { - calculatedEdge.normalize(); btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); calculatedNormalA.normalize(); - angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB); - ang4 = SIMD_PI-angle2; + angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB); + ang4 = SIMD_PI - angle2; btScalar dotA = normalA.dot(edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision - isConvex = (dotA<0.); + isConvex = (dotA < 0.); correctedAngle = isConvex ? ang4 : -ang4; } - - - - - //alternatively use + //alternatively use //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); - switch (sumvertsA) { - case 1: + case 1: { - btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); + btVector3 edge = m_triangleVerticesA[0] - m_triangleVerticesA[1]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); btScalar bla = computedNormalB.dot(normalB); - if (bla<0) + if (bla < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif//DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV0V1Angle = -correctedAngle; @@ -249,44 +237,44 @@ struct btConnectivityProcessor : public btTriangleCallback info->m_flags |= TRI_INFO_V0V1_CONVEX; break; } - case 2: + case 2: { - btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV2V0Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V2V0_CONVEX; - break; + break; } - case 3: + case 3: { - btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[1] - m_triangleVerticesA[2]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB; - computedNormalB*=-1; + computedNormalB *= -1; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV1V2Angle = -correctedAngle; if (isConvex) @@ -297,18 +285,50 @@ struct btConnectivityProcessor : public btTriangleCallback break; } - default: + default: { // printf("warning: duplicate triangle\n"); } - } } }; + + +struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback +{ + btHeightfieldTerrainShape* m_heightfieldShape; + btTriangleInfoMap* m_triangleInfoMap; + + + b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap) + :m_heightfieldShape(heightFieldShape), + m_triangleInfoMap(triangleInfoMap) + { + } + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btConnectivityProcessor connectivityProcessor; + connectivityProcessor.m_partIdA = partId; + connectivityProcessor.m_triangleIndexA = triangleIndex; + connectivityProcessor.m_triangleVerticesA = triangle; + connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap; + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMin.setMin(triangle[0]); + aabbMax.setMax(triangle[0]); + aabbMin.setMin(triangle[1]); + aabbMax.setMax(triangle[1]); + aabbMin.setMin(triangle[2]); + aabbMax.setMax(triangle[2]); + + m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); + } +}; ///////////////////////////////////////////////////////// ///////////////////////////////////////////////////////// -void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap) +void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap) { //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! if (trimeshShape->getTriangleInfoMap()) @@ -319,46 +339,51 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); const btVector3& meshScaling = meshInterface->getScaling(); - for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) + for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++) { - const unsigned char *vertexbase = 0; + const unsigned char* vertexbase = 0; int numverts = 0; PHY_ScalarType type = PHY_INTEGER; int stride = 0; - const unsigned char *indexbase = 0; + const unsigned char* indexbase = 0; int indexstride = 0; int numfaces = 0; PHY_ScalarType indicestype = PHY_INTEGER; //PHY_ScalarType indexType=0; btVector3 triangleVerts[3]; - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); - btVector3 aabbMin,aabbMax; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId); + btVector3 aabbMin, aabbMax; - for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++) { - unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ())); } } - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangleVerts[0]); aabbMax.setMax(triangleVerts[0]); aabbMin.setMin(triangleVerts[1]); @@ -370,131 +395,177 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle connectivityProcessor.m_partIdA = partId; connectivityProcessor.m_triangleIndexA = triangleIndex; connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; - connectivityProcessor.m_triangleInfoMap = triangleInfoMap; + connectivityProcessor.m_triangleInfoMap = triangleInfoMap; - trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax); + trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); } - } - } +void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap) +{ + + //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! + if (heightfieldShape->getTriangleInfoMap()) + return; + + heightfieldShape->setTriangleInfoMap(triangleInfoMap); + + //get all the triangles of the heightfield + btVector3 aabbMin, aabbMax; + + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + + b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap); + heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax); + +} // Given a point and a line segment (defined by two points), compute the closest point // in the line. Cap the point at the endpoints of the line segment. -void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) +void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) { - btVector3 lineDelta = line1 - line0; + btVector3 lineDelta = line1 - line0; // Handle degenerate lines - if ( lineDelta.fuzzyZero()) + if (lineDelta.fuzzyZero()) { nearestPoint = line0; } else { - btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta); + btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta); // Clamp the point to conform to the segment's endpoints - if ( delta < 0 ) + if (delta < 0) delta = 0; - else if ( delta > 1 ) + else if (delta > 1) delta = 1; - nearestPoint = line0 + lineDelta*delta; + nearestPoint = line0 + lineDelta * delta; } } - - - -bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal) +bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal) { btVector3 tri_normal = tri_normal_org; //we only have a local triangle normal, not a local contact normal -> only normal in world space... //either compute the current angle all in local space, or all in world space btVector3 edgeCross = edge.cross(tri_normal).normalize(); - btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB); + btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB); - if (correctedEdgeAngle<0) + if (correctedEdgeAngle < 0) { if (curAngle < correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } - if (correctedEdgeAngle>=0) + if (correctedEdgeAngle >= 0) { if (curAngle > correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } return false; } - - /// Changes a btManifoldPoint collision normal to the normal from the mesh. -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) { //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; + + btTriangleInfoMap* triangleInfoMapPtr = 0; + + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE) + { + btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + triangleInfoMapPtr = heightfield->getTriangleInfoMap(); + +//#define USE_HEIGHTFIELD_TRIANGLES +#ifdef USE_HEIGHTFIELD_TRIANGLES + btVector3 newNormal = btVector3(0, 0, 1); + + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); + btVector3 tri_normal; + tri_shape->calcNormal(tri_normal); + newNormal = tri_normal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + return; +#endif + } + + btBvhTriangleMeshShape* trimesh = 0; + + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); + } + else + { + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + } + } + if (trimesh) + { + triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap(); + } + - if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) - trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); - else - trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); - - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; - int hash = btGetHash(partId0,index0); - + int hash = btGetHash(partId0, index0); btTriangleInfo* info = triangleInfoMapPtr->find(hash); if (!info) return; - btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f; - + btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f; + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); - btVector3 v0,v1,v2; - tri_shape->getVertex(0,v0); - tri_shape->getVertex(1,v1); - tri_shape->getVertex(2,v2); + btVector3 v0, v1, v2; + tri_shape->getVertex(0, v0); + tri_shape->getVertex(1, v1); + tri_shape->getVertex(2, v2); //btVector3 center = (v0+v1+v2)*btScalar(1./3.); - btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0); + btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0); btVector3 tri_normal; tri_shape->calcNormal(tri_normal); //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); btVector3 nearest; - btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest); + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); btVector3 contact = cp.m_localPointB; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW const btTransform& tr = colObj0->getWorldTransform(); - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW bool isNearEdge = false; @@ -502,334 +573,325 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr int numConvexEdgeHits = 0; btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - localContactNormalOnB.normalize();//is this necessary? - + localContactNormalOnB.normalize(); //is this necessary? + // Get closest edge - int bestedge=-1; - btScalar disttobestedge=BT_LARGE_FLOAT; + int bestedge = -1; + btScalar disttobestedge = BT_LARGE_FLOAT; // // Edge 0 -> 1 - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=0; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 0; + disttobestedge = len; + } + } // Edge 1 -> 2 - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=1; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 1; + disttobestedge = len; + } + } // Edge 2 -> 0 - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=2; - disttobestedge=len; - } - } - + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 2; + disttobestedge = len; + } + } + #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); - btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); -#endif - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f); + btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red); +#endif + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); #endif - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==0 ) - { - btVector3 edge(v0-v1); - isNearEdge = true; - - if (info->m_edgeV0V1Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 0) { + btVector3 edge(v0 - v1); + isNearEdge = true; - bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW + if (info->m_edgeV0V1Angle == btScalar(0)) + { + numConcaveEdgeHits++; + } + else + { + bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 nA = swapFactor * tri_normal; + btVector3 nA = swapFactor * tri_normal; - btQuaternion orn(edge,info->m_edgeV0V1Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; + btQuaternion orn(edge, info->m_edgeV0V1Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); #ifdef DEBUG_INTERNAL_EDGE - { - - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE - if (backFacingNormal) - { - numConcaveEdgeHits++; - } - else - { - numConvexEdgeHits++; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); - if (isClamped) + if (backFacingNormal) { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. (what about cp.m_distance1?) - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); - + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v1,v2,nearest); + btNearestPointInLineSegment(contact, v1, v2, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); -#endif + btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green); +#endif - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - - - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==1 ) - { - isNearEdge = true; -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 edge(v1-v2); - - isNearEdge = true; + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if (info->m_edgeV1V2Angle == btScalar(0)) + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 1) { - numConcaveEdgeHits++; - } else - { - bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - - btQuaternion orn(edge,info->m_edgeV1V2Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btVector3 edge(v1 - v2); - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + isNearEdge = true; - if (backFacingNormal) + if (info->m_edgeV1V2Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); - if (isClamped) + bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + + btQuaternion orn(edge, info->m_edgeV1V2Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConvexEdgeHits++; + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v2,v0,nearest); + btNearestPointInLineSegment(contact, v2, v0, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); -#endif + btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue); +#endif - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { - -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==2 ) - { - isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 edge(v2-v0); + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if (info->m_edgeV2V0Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 2) { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - btQuaternion orn(edge,info->m_edgeV2V0Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + btVector3 edge(v2 - v0); - if (backFacingNormal) + if (info->m_edgeV2V0Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - // printf("hitting convex edge\n"); + bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + btQuaternion orn(edge, info->m_edgeV2V0Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); - if (isClamped) + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConvexEdgeHits++; + // printf("hitting convex edge\n"); + + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } - } + } } - - - } } #ifdef DEBUG_INTERNAL_EDGE { - btVector3 color(0,1,1); - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); + btVector3 color(0, 1, 1); + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { - - if (numConcaveEdgeHits>0) + if (numConcaveEdgeHits > 0) { - if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0) + if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0) { //fix tri_normal so it pointing the same direction as the current local contact normal if (tri_normal.dot(localContactNormalOnB) < 0) { tri_normal *= -1; } - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal; - } else + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal; + } + else { - btVector3 newNormal = tri_normal *frontFacing; + btVector3 newNormal = tri_normal * frontFacing; //if the tri_normal is pointing opposite direction as the current local contact normal, skip it - btScalar d = newNormal.dot(localContactNormalOnB) ; - if (d< 0) + btScalar d = newNormal.dot(localContactNormalOnB); + if (d < 0) { return; } //modify the normal to be the triangle normal (or backfacing normal) - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal; + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal; } - + // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h index 7d9aafeee69..cc6d11c2418 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -15,33 +15,30 @@ class btCollisionObject; struct btCollisionObjectWrapper; class btManifoldPoint; class btIDebugDraw; - - +class btHeightfieldTerrainShape; enum btInternalEdgeAdjustFlags { BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1, - BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended + BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4 }; - ///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo' -void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap); +void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap); +void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* trimeshShape, btTriangleInfoMap* triangleInfoMap); ///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo) ///If this info map is missing, or the triangle is not store in this map, nothing will be done -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap,const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); ///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly. ///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap //#define BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW -void btSetDebugDrawer(btIDebugDraw* debugDrawer); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - -#endif //BT_INTERNAL_EDGE_UTILITY_H +void btSetDebugDrawer(btIDebugDraw* debugDrawer); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW +#endif //BT_INTERNAL_EDGE_UTILITY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 4b2986a0087..770eb243691 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -13,142 +13,191 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btManifoldResult.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" ///This is to allow MaterialCombiner/Custom Friction/Restitution values -ContactAddedCallback gContactAddedCallback=0; - +ContactAddedCallback gContactAddedCallback = 0; +CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution; +CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction; +CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction; +CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction; +CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping; +CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness; -///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1) { - btScalar friction = body0->getRollingFriction() * body1->getRollingFriction(); + btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction(); - const btScalar MAX_FRICTION = btScalar(10.); + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) friction = MAX_FRICTION; return friction; - } +btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1) +{ + btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction(); + + const btScalar MAX_FRICTION = btScalar(10.); + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; +} ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1) { btScalar friction = body0->getFriction() * body1->getFriction(); - const btScalar MAX_FRICTION = btScalar(10.); + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) friction = MAX_FRICTION; return friction; - } -btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1) { return body0->getRestitution() * body1->getRestitution(); } +btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1) +{ + return body0->getContactDamping() + body1->getContactDamping(); +} + +btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1) +{ + btScalar s0 = body0->getContactStiffness(); + btScalar s1 = body1->getContactStiffness(); + btScalar tmp0 = btScalar(1) / s0; + btScalar tmp1 = btScalar(1) / s1; + btScalar combinedStiffness = btScalar(1) / (tmp0 + tmp1); + return combinedStiffness; +} -btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - :m_manifoldPtr(0), - m_body0Wrap(body0Wrap), - m_body1Wrap(body1Wrap) +btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : m_manifoldPtr(0), + m_body0Wrap(body0Wrap), + m_body1Wrap(body1Wrap) #ifdef DEBUG_PART_INDEX - ,m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX + , + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + , + m_closestPointDistanceThreshold(0) { } - -void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) +void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { btAssert(m_manifoldPtr); //order in manifold needs to match if (depth > m_manifoldPtr->getContactBreakingThreshold()) -// if (depth > m_manifoldPtr->getContactProcessingThreshold()) + // if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + bool isNewCollision = m_manifoldPtr->getNumContacts() == 0; btVector3 pointA = pointInWorld + normalOnBInWorld * depth; btVector3 localA; btVector3 localB; - + if (isSwapped) { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else + } + else { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + 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_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); - + newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); - - //BP mod, store contact triangles. + if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)) + { + newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING; + } + + if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR)) + { + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR; + } + + btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2); + + //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; - newPt.m_index0 = m_index1; - newPt.m_index1 = m_index0; - } else + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } + else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; } //printf("depth=%f\n",depth); ///@todo, check this for any side effects if (insertIndex >= 0) { //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); - m_manifoldPtr->replaceContactPoint(newPt,insertIndex); - } else + 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 - ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || - (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) + ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) { //experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; - (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); + const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; + (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1); } + if (gContactStartedCallback && isNewCollision) + { + gContactStartedCallback(m_manifoldPtr); + } } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 977b9a02fc5..6c0a2d9a43f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_MANIFOLD_RESULT_H #define BT_MANIFOLD_RESULT_H @@ -29,74 +28,81 @@ class btManifoldPoint; #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1); -extern ContactAddedCallback gContactAddedCallback; +typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1); +extern ContactAddedCallback gContactAddedCallback; //#define DEBUG_PART_INDEX 1 +/// These callbacks are used to customize the algorith that combine restitution, friction, damping, Stiffness +typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0, const btCollisionObject* body1); + +extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback; +extern CalculateCombinedCallback gCalculateCombinedFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback; +extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback; ///btManifoldResult is a helper class to manage contact results. class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result { protected: - btPersistentManifold* m_manifoldPtr; const btCollisionObjectWrapper* m_body0Wrap; const btCollisionObjectWrapper* m_body1Wrap; - int m_partId0; + int m_partId0; int m_partId1; int m_index0; int m_index1; - public: - btManifoldResult() -#ifdef DEBUG_PART_INDEX : - m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX +#ifdef DEBUG_PART_INDEX + + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + m_closestPointDistanceThreshold(0) { } - btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); - virtual ~btManifoldResult() {}; + virtual ~btManifoldResult(){}; - void setPersistentManifold(btPersistentManifold* manifoldPtr) + void setPersistentManifold(btPersistentManifold* manifoldPtr) { m_manifoldPtr = manifoldPtr; } - const btPersistentManifold* getPersistentManifold() const + const btPersistentManifold* getPersistentManifold() const { return m_manifoldPtr; } - btPersistentManifold* getPersistentManifold() + btPersistentManifold* getPersistentManifold() { return m_manifoldPtr; } - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { - m_partId0=partId0; - m_index0=index0; + m_partId0 = partId0; + m_index0 = index0; } - virtual void setShapeIdentifiersB( int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { - m_partId1=partId1; - m_index1=index1; + m_partId1 = partId1; + m_index1 = index1; } + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth); - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); - - SIMD_FORCE_INLINE void refreshContactPoints() + SIMD_FORCE_INLINE void refreshContactPoints() { btAssert(m_manifoldPtr); if (!m_manifoldPtr->getNumContacts()) @@ -106,10 +112,11 @@ public: if (isSwapped) { - m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform()); - } else + m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(), m_body0Wrap->getCollisionObject()->getWorldTransform()); + } + else { - m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform()); + m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(), m_body1Wrap->getCollisionObject()->getWorldTransform()); } } @@ -142,9 +149,15 @@ public: return m_body1Wrap->getCollisionObject(); } + btScalar m_closestPointDistanceThreshold; + /// in the future we can let the user override the methods to combine restitution and friction - static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1); - static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1); }; -#endif //BT_MANIFOLD_RESULT_H +#endif //BT_MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 13447822571..327b3f076a6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -14,7 +14,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btScalar.h" #include "btSimulationIslandManager.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -25,8 +24,7 @@ subject to the following restrictions: //#include <stdio.h> #include "LinearMath/btQuickprof.h" -btSimulationIslandManager::btSimulationIslandManager(): -m_splitIslands(true) +btSimulationIslandManager::btSimulationIslandManager() : m_splitIslands(true) { } @@ -34,53 +32,47 @@ btSimulationIslandManager::~btSimulationIslandManager() { } - void btSimulationIslandManager::initUnionFind(int n) { - m_unionFind.reset(n); + m_unionFind.reset(n); } - -void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) +void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */, btCollisionWorld* colWorld) { - { btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); if (numOverlappingPairs) { - btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - - for (int i=0;i<numOverlappingPairs;i++) - { - const btBroadphasePair& collisionPair = pairPtr[i]; - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + for (int i = 0; i < numOverlappingPairs; i++) { + const btBroadphasePair& collisionPair = pairPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - m_unionFind.unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + m_unionFind.unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } } } - } } } #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher) { - - // put the index into m_controllers into m_tag + // put the index into m_controllers into m_tag int index = 0; { - int i; - for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; //Adding filtering here if (!collisionObject->isStaticOrKinematicObject()) { @@ -92,28 +84,29 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWor } // do the union find - initUnionFind( index ); + initUnionFind(index); - findUnions(dispatcher,colWorld); + findUnions(dispatcher, colWorld); } -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { - // put the islandId ('find' value) into m_tag + // put the islandId ('find' value) into m_tag { int index = 0; int i; - for (i=0;i<colWorld->getCollisionObjectArray().size();i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; if (!collisionObject->isStaticOrKinematicObject()) { - collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setIslandTag(m_unionFind.find(index)); //Set the correct object offset in Collision Object Array m_unionFind.getElement(index).m_sz = i; collisionObject->setCompanionId(-1); index++; - } else + } + else { collisionObject->setIslandTag(-1); collisionObject->setCompanionId(-2); @@ -122,49 +115,44 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* c } } - -#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher) { + initUnionFind(int(colWorld->getCollisionObjectArray().size())); - initUnionFind( int (colWorld->getCollisionObjectArray().size())); - - // put the index into m_controllers into m_tag + // put the index into m_controllers into m_tag { - int index = 0; int i; - for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; collisionObject->setIslandTag(index); collisionObject->setCompanionId(-1); collisionObject->setHitFraction(btScalar(1.)); index++; - } } // do the union find - findUnions(dispatcher,colWorld); + findUnions(dispatcher, colWorld); } -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { - // put the islandId ('find' value) into m_tag + // put the islandId ('find' value) into m_tag { - - int index = 0; int i; - for (i=0;i<colWorld->getCollisionObjectArray().size();i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; if (!collisionObject->isStaticOrKinematicObject()) { - collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setIslandTag(m_unionFind.find(index)); collisionObject->setCompanionId(-1); - } else + } + else { collisionObject->setIslandTag(-1); collisionObject->setCompanionId(-2); @@ -174,56 +162,59 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col } } -#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION -inline int getIslandId(const btPersistentManifold* lhs) +inline int getIslandId(const btPersistentManifold* lhs) { int islandId; const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); - islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); + islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); return islandId; - } - - /// function object that routes calls to operator< class btPersistentManifoldSortPredicate { - public: - - SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const - { - return getIslandId(lhs) < getIslandId(rhs); - } +public: + SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + { + return getIslandId(lhs) < getIslandId(rhs); + } }; - -void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) +class btPersistentManifoldSortPredicateDeterministic { +public: + SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + { + return ( + (getIslandId(lhs) < getIslandId(rhs)) || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) || ((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))); + } +}; +void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld) +{ BT_PROFILE("islandUnionFindAndQuickSort"); - + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); m_islandmanifold.resize(0); //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 - + getUnionFind().sortIslands(); int numElem = getUnionFind().getNumElements(); - int endIslandIndex=1; + int endIslandIndex = 1; int startIslandIndex; - //update the sleeping state for bodies, if all are sleeping - for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) { int islandId = getUnionFind().getElement(startIslandIndex).m_id; - for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) + for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) { } @@ -232,71 +223,69 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio bool allSleeping = true; int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if (colObj0->getActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->getActivationState()== DISABLE_DEACTIVATION) + if (colObj0->getActivationState() == ACTIVE_TAG || + colObj0->getActivationState() == DISABLE_DEACTIVATION) { allSleeping = false; + break; } } } - if (allSleeping) { int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - colObj0->setActivationState( ISLAND_SLEEPING ); + colObj0->setActivationState(ISLAND_SLEEPING); } } - } else + } + else { - int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) + if (colObj0->getActivationState() == ISLAND_SLEEPING) { - colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setActivationState(WANTS_DEACTIVATION); colObj0->setDeactivationTime(0.f); } } @@ -304,29 +293,30 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio } } - int i; int maxNumManifolds = dispatcher->getNumManifolds(); -//#define SPLIT_ISLANDS 1 -//#ifdef SPLIT_ISLANDS + //#define SPLIT_ISLANDS 1 + //#ifdef SPLIT_ISLANDS - -//#endif //SPLIT_ISLANDS + //#endif //SPLIT_ISLANDS - - for (i=0;i<maxNumManifolds ;i++) + for (i = 0; i < maxNumManifolds; i++) { - btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); - - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); - const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); - - ///@todo: check sleeping conditions! - if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + if (manifold->getNumContacts() == 0) + continue; + } + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) { - //kinematic objects don't merge islands, but wake up all connected objects if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) { @@ -338,10 +328,10 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio if (colObj1->hasContactResponse()) colObj0->activate(); } - if(m_splitIslands) - { + if (m_splitIslands) + { //filtering for response - if (dispatcher->needsResponse(colObj0,colObj1)) + if (dispatcher->needsResponse(colObj0, colObj1)) m_islandmanifold.push_back(manifold); } } @@ -349,25 +339,27 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio } - ///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback) { - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - buildIslands(dispatcher,collisionWorld); + buildIslands(dispatcher, collisionWorld); + processIslands(dispatcher, collisionWorld, callback); +} - int endIslandIndex=1; +void btSimulationIslandManager::processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback) +{ + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + int endIslandIndex = 1; int startIslandIndex; int numElem = getUnionFind().getNumElements(); BT_PROFILE("processIslands"); - if(!m_splitIslands) + if (!m_splitIslands) { btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); int maxNumManifolds = dispatcher->getNumManifolds(); - callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1); } else { @@ -375,11 +367,21 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, // Sort the vector using predicate and std::sort //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); - int numManifolds = int (m_islandmanifold.size()); + int numManifolds = int(m_islandmanifold.size()); //tried a radix sort, but quicksort/heapsort seems still faster //@todo rewrite island management - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid, + //but also based on object0 unique id and object1 unique id + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); + } + else + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + } + //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) @@ -389,55 +391,49 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //int islandId; - - - // printf("Start Islands\n"); + // 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) + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) { int islandId = getUnionFind().getElement(startIslandIndex).m_id; + bool islandSleeping = true; - bool islandSleeping = true; - - for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) - { - int i = getUnionFind().getElement(endIslandIndex).m_sz; - btCollisionObject* colObj0 = collisionObjects[i]; - m_islandBodies.push_back(colObj0); - if (colObj0->isActive()) - islandSleeping = false; - } - + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) + { + int i = getUnionFind().getElement(endIslandIndex).m_sz; + btCollisionObject* colObj0 = collisionObjects[i]; + m_islandBodies.push_back(colObj0); + if (colObj0->isActive()) + islandSleeping = false; + } //find the accompanying contact manifold for this islandId int numIslandManifolds = 0; btPersistentManifold** startManifold = 0; - if (startManifoldIndex<numManifolds) + if (startManifoldIndex < numManifolds) { int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]); if (curIslandId == islandId) { startManifold = &m_islandmanifold[startManifoldIndex]; - - for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) - { + for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++) + { } /// Process the actual simulation, only if not sleeping/deactivated - numIslandManifolds = endManifoldIndex-startManifoldIndex; + numIslandManifolds = endManifoldIndex - startManifoldIndex; } - } if (!islandSleeping) { - callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); - // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); + 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) { startManifoldIndex = endManifoldIndex; @@ -445,6 +441,5 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, m_islandBodies.resize(0); } - } // else if(!splitIslands) - + } // else if(!splitIslands) } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index e24c6afeca1..197bb457cf6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -26,46 +26,42 @@ 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; - + btAlignedObjectArray<btPersistentManifold*> m_islandmanifold; + btAlignedObjectArray<btCollisionObject*> m_islandBodies; + bool m_splitIslands; - + public: btSimulationIslandManager(); virtual ~btSimulationIslandManager(); + void initUnionFind(int n); - void initUnionFind(int n); - - - btUnionFind& getUnionFind() { return m_unionFind;} - - virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher); - virtual void storeIslandActivationState(btCollisionWorld* world); + btUnionFind& getUnionFind() { return m_unionFind; } + virtual void updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher); + virtual void storeIslandActivationState(btCollisionWorld* world); - void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld); + void findUnions(btDispatcher* dispatcher, btCollisionWorld* colWorld); - - - struct IslandCallback + struct IslandCallback { - virtual ~IslandCallback() {}; + virtual ~IslandCallback(){}; - virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void processIsland(btCollisionObject** bodies, int numBodies, class btPersistentManifold** manifolds, int numManifolds, int islandId) = 0; }; - void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); - - void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + void buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); + + void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld); + void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); + bool getSplitIslands() { return m_splitIslands; @@ -74,8 +70,6 @@ public: { m_splitIslands = doSplitIslands; } - }; -#endif //BT_SIMULATION_ISLAND_MANAGER_H - +#endif //BT_SIMULATION_ISLAND_MANAGER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index e8b567e0efc..bc68b285b8d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -21,23 +21,22 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" //#include <stdio.h> -btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped) -: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped) +btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf), + m_isSwapped(isSwapped) { - const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap; - - if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject())) + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? col0Wrap : col1Wrap; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject()); m_ownManifold = true; } } - btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() { if (m_ownManifold) @@ -47,17 +46,15 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() } } - - -void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) +void btSphereBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; (void)resultOut; if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap; + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? body0Wrap : body1Wrap; btVector3 pOnBox; @@ -83,10 +80,9 @@ void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWra resultOut->refreshContactPoints(); } } - } -btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -97,27 +93,26 @@ btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* return btScalar(1.); } - -bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) +bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance) { - const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape(); - btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); + const btBoxShape* boxShape = (const btBoxShape*)boxObjWrap->getCollisionShape(); + btVector3 const& boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); btScalar boxMargin = boxShape->getMargin(); penetrationDepth = 1.0f; // convert the sphere position to the box's local space - btTransform const &m44T = boxObjWrap->getWorldTransform(); + btTransform const& m44T = boxObjWrap->getWorldTransform(); btVector3 sphereRelPos = m44T.invXform(sphereCenter); // Determine the closest point to the sphere center in the box btVector3 closestPoint = sphereRelPos; - closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) ); - closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) ); - closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) ); - closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) ); - closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) ); - closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) ); - + closestPoint.setX(btMin(boxHalfExtent.getX(), closestPoint.getX())); + closestPoint.setX(btMax(-boxHalfExtent.getX(), closestPoint.getX())); + closestPoint.setY(btMin(boxHalfExtent.getY(), closestPoint.getY())); + closestPoint.setY(btMax(-boxHalfExtent.getY(), closestPoint.getY())); + closestPoint.setZ(btMin(boxHalfExtent.getZ(), closestPoint.getZ())); + closestPoint.setZ(btMax(-boxHalfExtent.getZ(), closestPoint.getZ())); + btScalar intersectionDist = fRadius + boxMargin; btScalar contactDist = intersectionDist + maxContactDistance; normal = sphereRelPos - closestPoint; @@ -136,42 +131,42 @@ bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWra { distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); } - else //compute the penetration details + else //compute the penetration details { distance = normal.length(); normal /= distance; } pointOnBox = closestPoint + normal * boxMargin; -// v3PointOnSphere = sphereRelPos - (normal * fRadius); + // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; // transform back in world space btVector3 tmp = m44T(pointOnBox); pointOnBox = tmp; -// tmp = m44T(v3PointOnSphere); -// v3PointOnSphere = tmp; + // tmp = m44T(v3PointOnSphere); + // v3PointOnSphere = tmp; tmp = m44T.getBasis() * normal; normal = tmp; return true; } -btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) +btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal) { //project the center of the sphere on the closest face of the box btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX(); btScalar minDist = faceDist; - closestPoint.setX( boxHalfExtent.getX() ); - normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f)); + closestPoint.setX(boxHalfExtent.getX()); + normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f)); faceDist = boxHalfExtent.getX() + sphereRelPos.getX(); if (faceDist < minDist) { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setX( -boxHalfExtent.getX() ); - normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f)); + closestPoint.setX(-boxHalfExtent.getX()); + normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getY() - sphereRelPos.getY(); @@ -179,8 +174,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setY( boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); + closestPoint.setY(boxHalfExtent.getY()); + normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getY() + sphereRelPos.getY(); @@ -188,8 +183,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setY( -boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); + closestPoint.setY(-boxHalfExtent.getY()); + normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ(); @@ -197,8 +192,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setZ( boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); + closestPoint.setZ(boxHalfExtent.getZ()); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); } faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ(); @@ -206,8 +201,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setZ( -boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); + closestPoint.setZ(-boxHalfExtent.getZ()); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); } return minDist; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index eefaedc9e7e..3348bc89af5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -28,21 +28,20 @@ class btPersistentManifold; /// Other features are frame-coherency (persistent data) and collision response. class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - -public: + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; - btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped); +public: + btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btSphereBoxCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -50,26 +49,25 @@ public: } } - bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance ); + bool getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance); + + btScalar getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal); - btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false); - } else + return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false); + } + else { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true); + return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true); } } }; - }; -#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 36ba21f5bb0..7fa0559f975 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -12,6 +12,7 @@ subject to the following restrictions: 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. */ +#define CLEAR_MANIFOLD 1 #include "btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -19,14 +20,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) -: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap) + : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { if (!m_manifoldPtr) { - m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(), col1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -40,7 +41,7 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() } } -void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSphereSphereCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; @@ -52,27 +53,27 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape(); btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape(); - btVector3 diff = col0Wrap->getWorldTransform().getOrigin()- col1Wrap->getWorldTransform().getOrigin(); + btVector3 diff = col0Wrap->getWorldTransform().getOrigin() - col1Wrap->getWorldTransform().getOrigin(); btScalar len = diff.length(); btScalar radius0 = sphere0->getRadius(); btScalar radius1 = sphere1->getRadius(); #ifdef CLEAR_MANIFOLD - m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting + m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting #endif ///iff distance positive, don't generate a new contact - if ( len > (radius0+radius1)) + if (len > (radius0 + radius1 + resultOut->m_closestPointDistanceThreshold)) { #ifndef CLEAR_MANIFOLD resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD +#endif //CLEAR_MANIFOLD return; } ///distance (negative means penetration) - btScalar dist = len - (radius0+radius1); + btScalar dist = len - (radius0 + radius1); - btVector3 normalOnSurfaceB(1,0,0); + btVector3 normalOnSurfaceB(1, 0, 0); if (len > SIMD_EPSILON) { normalOnSurfaceB = diff / len; @@ -81,20 +82,18 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject ///point on A (worldspace) ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) - btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; + btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1 * normalOnSurfaceB; /// report a contact. internally this will be kept persistent, and contact reduction is done - - - resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); + + resultOut->addContactPoint(normalOnSurfaceB, pos1, dist); #ifndef CLEAR_MANIFOLD resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD - +#endif //CLEAR_MANIFOLD } -btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)col0; (void)col1; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 3517a568a99..b08d0df76d8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -28,39 +28,37 @@ class btPersistentManifold; /// Also provides the most basic sample for custom/user btCollisionAlgorithm class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: - btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap); + btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap); btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { manifoldArray.push_back(m_manifoldPtr); } } - + virtual ~btSphereSphereCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); - return new(mem) btSphereSphereCollisionAlgorithm(0,ci,col0Wrap,col1Wrap); + return new (mem) btSphereSphereCollisionAlgorithm(0, ci, col0Wrap, col1Wrap); } }; - }; -#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 280a4d355fd..1bc3056c013 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSphereTriangleCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -21,15 +20,15 @@ subject to the following restrictions: #include "SphereTriangleDetector.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf), -m_swapped(swapped) +btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf), + m_swapped(swapped) { if (!m_manifoldPtr) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -43,36 +42,35 @@ btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() } } -void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSphereTriangleCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* sphereObjWrap = m_swapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* triObjWrap = m_swapped? col0Wrap : col1Wrap; + const btCollisionObjectWrapper* sphereObjWrap = m_swapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* triObjWrap = m_swapped ? col0Wrap : col1Wrap; btSphereShape* sphere = (btSphereShape*)sphereObjWrap->getCollisionShape(); btTriangleShape* triangle = (btTriangleShape*)triObjWrap->getCollisionShape(); - + /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); - + SphereTriangleDetector detector(sphere, triangle, m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold); + btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); ///@todo: tighter bounds input.m_transformA = sphereObjWrap->getWorldTransform(); input.m_transformB = triObjWrap->getWorldTransform(); bool swapResults = m_swapped; - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); + 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) +btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 6b6e39a72b6..d660222f164 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -27,43 +27,39 @@ class btPersistentManifold; /// Also provides the most basic sample for custom/user btCollisionAlgorithm class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_swapped; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_swapped; + public: - btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped); + btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped); btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(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) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { manifoldArray.push_back(m_manifoldPtr); } } - + virtual ~btSphereTriangleCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); - return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_swapped); + return new (mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_swapped); } }; - }; -#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 5222933595d..816bf1e6adc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -15,68 +15,60 @@ subject to the following restrictions: #include "btUnionFind.h" - - btUnionFind::~btUnionFind() { Free(); - } btUnionFind::btUnionFind() -{ - +{ } -void btUnionFind::allocate(int N) +void btUnionFind::allocate(int N) { m_elements.resize(N); } -void btUnionFind::Free() +void btUnionFind::Free() { m_elements.clear(); } - -void btUnionFind::reset(int N) +void btUnionFind::reset(int N) { allocate(N); - for (int i = 0; i < N; i++) - { - m_elements[i].m_id = i; m_elements[i].m_sz = 1; - } + for (int i = 0; i < N; i++) + { + m_elements[i].m_id = i; + m_elements[i].m_sz = 1; + } } - class btUnionFindElementSortPredicate { - public: - - bool operator() ( const btElement& lhs, const btElement& rhs ) const - { - return lhs.m_id < rhs.m_id; - } +public: + bool operator()(const btElement& lhs, const btElement& rhs) const + { + return lhs.m_id < rhs.m_id; + } }; ///this is a special operation, destroying the content of btUnionFind. ///it sorts the elements, based on island id, in order to make it easy to iterate over islands -void btUnionFind::sortIslands() +void btUnionFind::sortIslands() { - //first store the original body index, and islandId int numElements = m_elements.size(); - - for (int i=0;i<numElements;i++) + + for (int i = 0; i < numElements; i++) { m_elements[i].m_id = find(i); #ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION m_elements[i].m_sz = i; -#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION } - - // Sort the vector using predicate and std::sort - //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); - m_elements.quickSort(btUnionFindElementSortPredicate()); + // Sort the vector using predicate and std::sort + //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + m_elements.quickSort(btUnionFindElementSortPredicate()); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index ef2a29202f7..d422ef55eb3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -23,107 +23,101 @@ subject to the following restrictions: ///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406 #define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1 -struct btElement +struct btElement { - int m_id; - int m_sz; + int m_id; + int m_sz; }; ///UnionFind calculates connected subsets // Implements weighted Quick Union with path compression // optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable) class btUnionFind - { - private: - btAlignedObjectArray<btElement> m_elements; - - public: - - btUnionFind(); - ~btUnionFind(); - - - //this is a special operation, destroying the content of btUnionFind. - //it sorts the elements, based on island id, in order to make it easy to iterate over islands - void sortIslands(); - - void reset(int N); - - SIMD_FORCE_INLINE int getNumElements() const - { - return int(m_elements.size()); - } - SIMD_FORCE_INLINE bool isRoot(int x) const - { - return (x == m_elements[x].m_id); - } - - btElement& getElement(int index) - { - return m_elements[index]; - } - const btElement& getElement(int index) const - { - return m_elements[index]; - } - - void allocate(int N); - void Free(); - - - - - int find(int p, int q) - { - return (find(p) == find(q)); - } - - void unite(int p, int q) - { - int i = find(p), j = find(q); - if (i == j) - return; +{ +private: + btAlignedObjectArray<btElement> m_elements; + +public: + btUnionFind(); + ~btUnionFind(); + + //this is a special operation, destroying the content of btUnionFind. + //it sorts the elements, based on island id, in order to make it easy to iterate over islands + void sortIslands(); + + void reset(int N); + + SIMD_FORCE_INLINE int getNumElements() const + { + return int(m_elements.size()); + } + SIMD_FORCE_INLINE bool isRoot(int x) const + { + return (x == m_elements[x].m_id); + } + + btElement& getElement(int index) + { + return m_elements[index]; + } + const btElement& getElement(int index) const + { + return m_elements[index]; + } + + void allocate(int N); + void Free(); + + int find(int p, int q) + { + return (find(p) == find(q)); + } + + void unite(int p, int q) + { + int i = find(p), j = find(q); + if (i == j) + return; #ifndef USE_PATH_COMPRESSION - //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) - if (m_elements[i].m_sz < m_elements[j].m_sz) - { - m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; - } - else - { - m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; - } -#else - m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; -#endif //USE_PATH_COMPRESSION + //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + if (m_elements[i].m_sz < m_elements[j].m_sz) + { + m_elements[i].m_id = j; + m_elements[j].m_sz += m_elements[i].m_sz; + } + else + { + m_elements[j].m_id = i; + m_elements[i].m_sz += m_elements[j].m_sz; } +#else + m_elements[i].m_id = j; + m_elements[j].m_sz += m_elements[i].m_sz; +#endif //USE_PATH_COMPRESSION + } + + int find(int x) + { + //btAssert(x < m_N); + //btAssert(x >= 0); - int find(int x) - { + while (x != m_elements[x].m_id) + { + //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically + +#ifdef USE_PATH_COMPRESSION + const btElement* elementPtr = &m_elements[m_elements[x].m_id]; + m_elements[x].m_id = elementPtr->m_id; + x = elementPtr->m_id; +#else // + x = m_elements[x].m_id; +#endif //btAssert(x < m_N); //btAssert(x >= 0); - - while (x != m_elements[x].m_id) - { - //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically - - #ifdef USE_PATH_COMPRESSION - const btElement* elementPtr = &m_elements[m_elements[x].m_id]; - m_elements[x].m_id = elementPtr->m_id; - x = elementPtr->m_id; - #else// - x = m_elements[x].m_id; - #endif - //btAssert(x < m_N); - //btAssert(x >= 0); - - } - return x; } + return x; + } +}; - - }; - - -#endif //BT_UNION_FIND_H +#endif //BT_UNION_FIND_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp index ecce028c2e6..a3d8075dafa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp @@ -15,28 +15,23 @@ subject to the following restrictions: #include "btBox2dShape.h" +//{ -//{ - - -void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBox2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } - -void btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btBox2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h index ce333783e44..7e085f9e2ea 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -23,9 +23,9 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" ///The btBox2dShape 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. -ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btBox2dShape : public btPolyhedralConvexShape { - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead btVector3 m_centroid; @@ -33,122 +33,111 @@ ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape btVector3 m_normals[4]; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + 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 + return m_implicitShapeDimensions; //changed in Bullet 2.63: assume the scaling and margin are included } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { 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())); + 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 + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - for (int i=0;i<numVectors;i++) + + for (int i = 0; i < numVectors; i++) { const btVector3& vec = vectors[i]; supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - } - ///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero) - btBox2dShape( const btVector3& boxHalfExtents) + btBox2dShape(const btVector3& boxHalfExtents) : btPolyhedralConvexShape(), - m_centroid(0,0,0) + m_centroid(0, 0, 0) { - m_vertices[0].setValue(-boxHalfExtents.getX(),-boxHalfExtents.getY(),0); - m_vertices[1].setValue(boxHalfExtents.getX(),-boxHalfExtents.getY(),0); - m_vertices[2].setValue(boxHalfExtents.getX(),boxHalfExtents.getY(),0); - m_vertices[3].setValue(-boxHalfExtents.getX(),boxHalfExtents.getY(),0); + m_vertices[0].setValue(-boxHalfExtents.getX(), -boxHalfExtents.getY(), 0); + m_vertices[1].setValue(boxHalfExtents.getX(), -boxHalfExtents.getY(), 0); + m_vertices[2].setValue(boxHalfExtents.getX(), boxHalfExtents.getY(), 0); + m_vertices[3].setValue(-boxHalfExtents.getX(), boxHalfExtents.getY(), 0); - m_normals[0].setValue(0,-1,0); - m_normals[1].setValue(1,0,0); - m_normals[2].setValue(0,1,0); - m_normals[3].setValue(-1,0,0); + m_normals[0].setValue(0, -1, 0); + m_normals[1].setValue(1, 0, 0); + m_normals[2].setValue(0, 1, 0); + m_normals[3].setValue(-1, 0, 0); btScalar minDimension = boxHalfExtents.getX(); - if (minDimension>boxHalfExtents.getY()) + if (minDimension > boxHalfExtents.getY()) minDimension = boxHalfExtents.getY(); - setSafeMargin(minDimension); m_shapeType = BOX_2D_SHAPE_PROXYTYPE; - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; + + setSafeMargin(minDimension); }; virtual void setMargin(btScalar collisionMargin) { //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; + btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); + btVector3 newMargin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; - } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + 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) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - - - - int getVertexCount() const + int getVertexCount() const { return 4; } - virtual int getNumVertices()const + virtual int getNumVertices() const { return 4; } @@ -163,82 +152,70 @@ public: return &m_normals[0]; } - - - - - - - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const { //this plane might not be aligned... - btVector4 plane ; - getPlaneEquation(plane,i); - planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ()); + btVector4 plane; + getPlaneEquation(plane, i); + planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ()); planeSupport = localGetSupportingVertex(-planeNormal); } - const btVector3& getCentroid() const { return m_centroid; } - + virtual int getNumPlanes() const { return 6; - } - - + } virtual int getNumEdges() const { return 12; } - - virtual void getVertex(int i,btVector3& vtx) const + virtual void getVertex(int i, btVector3& vtx) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); vtx = btVector3( - halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), - halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1), - halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1), + halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1), + halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2)); } - - virtual void getPlaneEquation(btVector4& plane,int i) const + virtual void getPlaneEquation(btVector4 & plane, int i) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); switch (i) { - case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); - break; - case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); - break; - case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); - break; - case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); - break; - default: - btAssert(0); + case 0: + plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 1: + plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 2: + plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y()); + break; + case 3: + plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y()); + break; + case 4: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z()); + break; + case 5: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z()); + break; + default: + btAssert(0); } } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -246,126 +223,117 @@ public: switch (i) { - case 0: + case 0: edgeVert0 = 0; edgeVert1 = 1; - break; - case 1: + break; + case 1: edgeVert0 = 0; edgeVert1 = 2; - break; - case 2: - edgeVert0 = 1; - edgeVert1 = 3; - - break; - case 3: - edgeVert0 = 2; - edgeVert1 = 3; - break; - case 4: - edgeVert0 = 0; - edgeVert1 = 4; - break; - case 5: - edgeVert0 = 1; - edgeVert1 = 5; - - break; - case 6: - edgeVert0 = 2; - edgeVert1 = 6; - break; - case 7: - edgeVert0 = 3; - edgeVert1 = 7; - break; - case 8: - edgeVert0 = 4; - edgeVert1 = 5; - break; - case 9: - edgeVert0 = 4; - edgeVert1 = 6; - break; - case 10: - edgeVert0 = 5; - edgeVert1 = 7; - break; - case 11: - edgeVert0 = 6; - edgeVert1 = 7; - break; - default: - btAssert(0); - + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + btAssert(0); } - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); + getVertex(edgeVert0, pa); + getVertex(edgeVert1, pb); } - - - - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); //btScalar minDist = 2*tolerance; - - bool result = (pt.x() <= (halfExtents.x()+tolerance)) && - (pt.x() >= (-halfExtents.x()-tolerance)) && - (pt.y() <= (halfExtents.y()+tolerance)) && - (pt.y() >= (-halfExtents.y()-tolerance)) && - (pt.z() <= (halfExtents.z()+tolerance)) && - (pt.z() >= (-halfExtents.z()-tolerance)); - + + bool result = (pt.x() <= (halfExtents.x() + tolerance)) && + (pt.x() >= (-halfExtents.x() - tolerance)) && + (pt.y() <= (halfExtents.y() + tolerance)) && + (pt.y() >= (-halfExtents.y() - tolerance)) && + (pt.z() <= (halfExtents.z() + tolerance)) && + (pt.z() >= (-halfExtents.z() - tolerance)); + return result; } - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "Box2d"; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 6; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { switch (index) { - case 0: - penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - break; - case 1: - penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - break; - case 2: - penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - break; - case 3: - penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - break; - case 4: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - break; - case 5: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - break; - default: - btAssert(0); + case 0: + penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.)); + break; + case 1: + penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.)); + break; + case 2: + penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.)); + break; + case 3: + penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.)); + break; + case 4: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.)); + break; + case 5: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.)); + break; + default: + btAssert(0); } } - }; -#endif //BT_OBB_BOX_2D_SHAPE_H - - +#endif //BT_OBB_BOX_2D_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index 3859138f18a..cb91d023e40 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -14,38 +14,32 @@ subject to the following restrictions: */ #include "btBoxShape.h" -btBoxShape::btBoxShape( const btVector3& boxHalfExtents) -: btPolyhedralConvexShape() +btBoxShape::btBoxShape(const btVector3& boxHalfExtents) + : btPolyhedralConvexShape() { m_shapeType = BOX_SHAPE_PROXYTYPE; - setSafeMargin(boxHalfExtents); - - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; -}; - - + setSafeMargin(boxHalfExtents); +}; -void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBoxShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } - -void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btBoxShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index 715e3f2ab98..3c65505d5bb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -23,112 +23,102 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" ///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. -ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btBoxShape : public btPolyhedralConvexShape { - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead - public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; return halfExtents; } - + const btVector3& getHalfExtentsWithoutMargin() const { - return m_implicitShapeDimensions;//scaling is included, margin is not + return m_implicitShapeDimensions; //scaling is included, margin is not } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { 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())); + 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 + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - for (int i=0;i<numVectors;i++) + + for (int i = 0; i < numVectors; i++) { const btVector3& vec = vectors[i]; supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - } - - btBoxShape( const btVector3& boxHalfExtents); + btBoxShape(const btVector3& boxHalfExtents); virtual void setMargin(btScalar collisionMargin) { //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; + btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); + btVector3 newMargin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; - } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + 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 getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const { //this plane might not be aligned... - btVector4 plane ; - getPlaneEquation(plane,i); - planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ()); + btVector4 plane; + getPlaneEquation(plane, i); + planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ()); planeSupport = localGetSupportingVertex(-planeNormal); } - virtual int getNumPlanes() const { return 6; - } - - virtual int getNumVertices() const + } + + virtual int getNumVertices() const { return 8; } @@ -138,49 +128,46 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); return 12; } - - virtual void getVertex(int i,btVector3& vtx) const + virtual void getVertex(int i, btVector3& vtx) const { btVector3 halfExtents = getHalfExtentsWithMargin(); vtx = btVector3( - halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), - halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1), - halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1), + halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1), + halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2)); } - - virtual void getPlaneEquation(btVector4& plane,int i) const + virtual void getPlaneEquation(btVector4 & plane, int i) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); switch (i) { - case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); - break; - case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); - break; - case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); - break; - case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); - break; - default: - btAssert(0); + case 0: + plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 1: + plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 2: + plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y()); + break; + case 3: + plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y()); + break; + case 4: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z()); + break; + case 5: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z()); + break; + default: + btAssert(0); } } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -188,127 +175,117 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); switch (i) { - case 0: + case 0: edgeVert0 = 0; edgeVert1 = 1; - break; - case 1: + break; + case 1: edgeVert0 = 0; edgeVert1 = 2; - break; - case 2: - edgeVert0 = 1; - edgeVert1 = 3; - - break; - case 3: - edgeVert0 = 2; - edgeVert1 = 3; - break; - case 4: - edgeVert0 = 0; - edgeVert1 = 4; - break; - case 5: - edgeVert0 = 1; - edgeVert1 = 5; - - break; - case 6: - edgeVert0 = 2; - edgeVert1 = 6; - break; - case 7: - edgeVert0 = 3; - edgeVert1 = 7; - break; - case 8: - edgeVert0 = 4; - edgeVert1 = 5; - break; - case 9: - edgeVert0 = 4; - edgeVert1 = 6; - break; - case 10: - edgeVert0 = 5; - edgeVert1 = 7; - break; - case 11: - edgeVert0 = 6; - edgeVert1 = 7; - break; - default: - btAssert(0); - + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + btAssert(0); } - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); + getVertex(edgeVert0, pa); + getVertex(edgeVert1, pb); } - - - - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); //btScalar minDist = 2*tolerance; - - bool result = (pt.x() <= (halfExtents.x()+tolerance)) && - (pt.x() >= (-halfExtents.x()-tolerance)) && - (pt.y() <= (halfExtents.y()+tolerance)) && - (pt.y() >= (-halfExtents.y()-tolerance)) && - (pt.z() <= (halfExtents.z()+tolerance)) && - (pt.z() >= (-halfExtents.z()-tolerance)); - + + bool result = (pt.x() <= (halfExtents.x() + tolerance)) && + (pt.x() >= (-halfExtents.x() - tolerance)) && + (pt.y() <= (halfExtents.y() + tolerance)) && + (pt.y() >= (-halfExtents.y() - tolerance)) && + (pt.z() <= (halfExtents.z() + tolerance)) && + (pt.z() >= (-halfExtents.z() - tolerance)); + return result; } - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "Box"; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 6; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { switch (index) { - case 0: - penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - break; - case 1: - penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - break; - case 2: - penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - break; - case 3: - penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - break; - case 4: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - break; - case 5: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - break; - default: - btAssert(0); + case 0: + penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.)); + break; + case 1: + penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.)); + break; + case 2: + penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.)); + break; + case 3: + penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.)); + break; + case 4: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.)); + break; + case 5: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.)); + break; + default: + btAssert(0); } } - }; - -#endif //BT_OBB_BOX_MINKOWSKI_H - - +#endif //BT_OBB_BOX_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index ace4cfa2646..c66ce58e3ef 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -22,11 +22,11 @@ subject to the following restrictions: ///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, bool buildBvh) -:btTriangleMeshShape(meshInterface), -m_bvh(0), -m_triangleInfoMap(0), -m_useQuantizedAabbCompression(useQuantizedAabbCompression), -m_ownsBvh(false) + : btTriangleMeshShape(meshInterface), + m_bvh(0), + m_triangleInfoMap(0), + m_useQuantizedAabbCompression(useQuantizedAabbCompression), + m_ownsBvh(false) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface @@ -37,16 +37,15 @@ m_ownsBvh(false) buildOptimizedBvh(); } -#endif //DISABLE_BVH - +#endif //DISABLE_BVH } -btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh) -:btTriangleMeshShape(meshInterface), -m_bvh(0), -m_triangleInfoMap(0), -m_useQuantizedAabbCompression(useQuantizedAabbCompression), -m_ownsBvh(false) +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh) + : btTriangleMeshShape(meshInterface), + m_bvh(0), + m_triangleInfoMap(0), + m_useQuantizedAabbCompression(useQuantizedAabbCompression), + m_ownsBvh(false) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface @@ -54,30 +53,28 @@ m_ownsBvh(false) if (buildBvh) { - void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16); m_bvh = new (mem) btOptimizedBvh(); - - m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + + m_bvh->build(meshInterface, m_useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax); m_ownsBvh = true; } -#endif //DISABLE_BVH - +#endif //DISABLE_BVH } -void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax) +void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax) { - m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax ); - + m_bvh->refitPartial(m_meshInterface, aabbMin, aabbMax); + m_localAabbMin.setMin(aabbMin); m_localAabbMax.setMax(aabbMax); } - -void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax) +void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin, const btVector3& aabbMax) { - m_bvh->refit( m_meshInterface, aabbMin,aabbMax ); - + m_bvh->refit(m_meshInterface, aabbMin, aabbMax); + recalcLocalAabb(); } @@ -90,27 +87,27 @@ btBvhTriangleMeshShape::~btBvhTriangleMeshShape() } } -void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) +void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) { - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; + btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(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; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; @@ -126,60 +123,65 @@ void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } + if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ()); } } /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); - m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); + m_bvh->reportRayOverlappingNodex(&myNodeCallback, raySource, rayTarget); } -void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) +void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) { - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; + btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(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; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; @@ -195,75 +197,79 @@ void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, co indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ()); } } /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); - m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); + m_bvh->reportBoxCastOverlappingNodex(&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); } //perform bvh tree traversal and report overlapping triangles to 'callback' -void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - #ifdef DISABLE_BVH //brute force traverse all triangles - btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax); + btTriangleMeshShape::processAllTriangles(callback, aabbMin, aabbMax); #else //first get all the nodes - - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; - btTriangleCallback* m_callback; - btVector3 m_triangle[3]; - + btStridingMeshInterface* m_meshInterface; + btTriangleCallback* m_callback; + btVector3 m_triangle[3]; + int m_numOverlap; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) + MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface) + : m_meshInterface(meshInterface), + m_callback(callback), + m_numOverlap(0) { } - + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { - const unsigned char *vertexbase; + m_numOverlap++; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; - m_meshInterface->getLockedReadOnlyVertexIndexBase( &vertexbase, @@ -276,68 +282,62 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT||indicestype==PHY_UCHAR); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT || indicestype == PHY_UCHAR); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:indicestype==PHY_INTEGER?gfxbase[j]:((unsigned char*)gfxbase)[j]; - + int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : indicestype == PHY_INTEGER ? gfxbase[j] : ((unsigned char*)gfxbase)[j]; #ifdef DEBUG_TRIANGLE_MESH - printf("%d ,",graphicsindex); -#endif //DEBUG_TRIANGLE_MESH + printf("%d ,", graphicsindex); +#endif //DEBUG_TRIANGLE_MESH if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + m_triangle[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); m_triangle[j] = btVector3( - btScalar(graphicsbase[0])*meshScaling.getX(), - btScalar(graphicsbase[1])*meshScaling.getY(), - btScalar(graphicsbase[2])*meshScaling.getZ()); + btScalar(graphicsbase[0]) * meshScaling.getX(), + btScalar(graphicsbase[1]) * meshScaling.getY(), + btScalar(graphicsbase[2]) * meshScaling.getZ()); } #ifdef DEBUG_TRIANGLE_MESH - printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z()); -#endif //DEBUG_TRIANGLE_MESH + printf("triangle vertices:%f,%f,%f\n", triangle[j].x(), triangle[j].y(), triangle[j].z()); +#endif //DEBUG_TRIANGLE_MESH } - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } - }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); - - m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - -#endif//DISABLE_BVH + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); + m_bvh->reportAabbOverlappingNodex(&myNodeCallback, aabbMin, aabbMax); +#endif //DISABLE_BVH } -void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - btTriangleMeshShape::setLocalScaling(scaling); - buildOptimizedBvh(); - } + if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + buildOptimizedBvh(); + } } -void btBvhTriangleMeshShape::buildOptimizedBvh() +void btBvhTriangleMeshShape::buildOptimizedBvh() { if (m_ownsBvh) { @@ -345,43 +345,39 @@ void btBvhTriangleMeshShape::buildOptimizedBvh() 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(); + 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_bvh->build(m_meshInterface, m_useQuantizedAabbCompression, m_localAabbMin, m_localAabbMax); m_ownsBvh = true; } -void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling) +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); - } -} - + 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); + } +} ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer; + btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*)dataBuffer; - btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer); m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); trimeshData->m_collisionMargin = float(m_collisionMargin); - - - if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH)) + if (m_bvh && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_BVH)) { void* chunk = serializer->findPointer(m_bvh); if (chunk) @@ -390,77 +386,77 @@ const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk; trimeshData->m_quantizedFloatBvh = 0; #else - trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk; - trimeshData->m_quantizedDoubleBvh= 0; -#endif //BT_USE_DOUBLE_PRECISION - } else + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk; + trimeshData->m_quantizedDoubleBvh = 0; +#endif //BT_USE_DOUBLE_PRECISION + } + else { - #ifdef BT_USE_DOUBLE_PRECISION trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); trimeshData->m_quantizedFloatBvh = 0; #else - trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); - trimeshData->m_quantizedDoubleBvh= 0; -#endif //BT_USE_DOUBLE_PRECISION - + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); + trimeshData->m_quantizedDoubleBvh = 0; +#endif //BT_USE_DOUBLE_PRECISION + int sz = m_bvh->calculateSerializeBufferSizeNew(); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh); + serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, m_bvh); } - } else + } + else { trimeshData->m_quantizedFloatBvh = 0; trimeshData->m_quantizedDoubleBvh = 0; } - - - if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP)) + if (m_triangleInfoMap && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_TRIANGLEINFOMAP)) { void* chunk = serializer->findPointer(m_triangleInfoMap); if (chunk) { trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk; - } else + } + else { trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap); int sz = m_triangleInfoMap->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap); + serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, m_triangleInfoMap); } - } else + } + else { trimeshData->m_triangleInfoMap = 0; } + // Fill padding with zeros to appease msan. + memset(trimeshData->m_pad3, 0, sizeof(trimeshData->m_pad3)); + return "btTriangleMeshShapeData"; } -void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const +void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const { if (m_bvh) { - int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place - btChunk* chunk = serializer->allocate(len,1); + int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place + btChunk* chunk = serializer->allocate(len, 1); const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh); + serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, (void*)m_bvh); } } -void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const +void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const { if (m_triangleInfoMap) { int len = m_triangleInfoMap->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap); + serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, (void*)m_triangleInfoMap); } } - - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 1fa4995d160..8b2f2ee85e8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -23,103 +23,99 @@ subject to the following restrictions: ///The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving objects. ///If you required moving concave triangle meshes, it is recommended to perform convex decomposition -///using HACD, see Bullet/Demos/ConvexDecompositionDemo. +///using HACD, see Bullet/Demos/ConvexDecompositionDemo. ///Alternatively, you can use btGimpactMeshShape for moving concave triangle meshes. -///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and -///cache friendly traversal for PlayStation 3 Cell SPU. +///btBvhTriangleMeshShape has 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 +ATTRIBUTE_ALIGNED16(class) +btBvhTriangleMeshShape : public btTriangleMeshShape { - - btOptimizedBvh* m_bvh; - btTriangleInfoMap* m_triangleInfoMap; + btOptimizedBvh* m_bvh; + btTriangleInfoMap* m_triangleInfoMap; bool m_useQuantizedAabbCompression; bool m_ownsBvh; #ifdef __clang__ - bool m_pad[11] __attribute__((unused));////need padding due to alignment + bool m_pad[11] __attribute__((unused)); ////need padding due to alignment #else - bool m_pad[11];////need padding due to alignment + bool m_pad[11]; ////need padding due to alignment #endif public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); + 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, bool buildBvh = true); - + btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh = true); + virtual ~btBvhTriangleMeshShape(); - bool getOwnsBvh () const + bool getOwnsBvh() const { return m_ownsBvh; } + 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); - - 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; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - void refitTree(const btVector3& aabbMin,const btVector3& aabbMax); + 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); + void partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax); //debugging - virtual const char* getName()const {return "BVHTRIANGLEMESH";} + virtual const char* getName() const { return "BVHTRIANGLEMESH"; } + virtual void setLocalScaling(const btVector3& scaling); - virtual void setLocalScaling(const btVector3& scaling); - - btOptimizedBvh* getOptimizedBvh() + btOptimizedBvh* getOptimizedBvh() { return m_bvh; } - void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1)); + void setOptimizedBvh(btOptimizedBvh * bvh, const btVector3& localScaling = btVector3(1, 1, 1)); - void buildOptimizedBvh(); + void buildOptimizedBvh(); - bool usesQuantizedAabbCompression() const + bool usesQuantizedAabbCompression() const { - return m_useQuantizedAabbCompression; + return m_useQuantizedAabbCompression; } - void setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap) + void setTriangleInfoMap(btTriangleInfoMap * triangleInfoMap) { m_triangleInfoMap = triangleInfoMap; } - const btTriangleInfoMap* getTriangleInfoMap() const + const btTriangleInfoMap* getTriangleInfoMap() const { return m_triangleInfoMap; } - - btTriangleInfoMap* getTriangleInfoMap() + + btTriangleInfoMap* getTriangleInfoMap() { return m_triangleInfoMap; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - virtual void serializeSingleBvh(btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - virtual void serializeSingleTriangleInfoMap(btSerializer* serializer) const; + virtual void serializeSingleBvh(btSerializer * serializer) const; + virtual void serializeSingleTriangleInfoMap(btSerializer * serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btTriangleMeshShapeData { @@ -138,12 +134,11 @@ struct btTriangleMeshShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const { return sizeof(btTriangleMeshShapeData); } - - -#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 864df26e931..7c337742841 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -13,24 +13,21 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () +btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape() { + m_collisionMargin = radius; m_shapeType = CAPSULE_SHAPE_PROXYTYPE; m_upAxis = 1; - m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); + m_implicitShapeDimensions.setValue(radius, 0.5f * height, radius); } - - btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -38,24 +35,22 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } btVector3 vtx; btScalar newDot; - - btScalar radius = getRadius(); - { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -64,10 +59,10 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } } { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -77,16 +72,11 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } return supVec; - } - void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - - - btScalar radius = getRadius(); - - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); const btVector3& vec = vectors[j]; @@ -94,9 +84,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btVector3 vtx; btScalar newDot; { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -105,9 +95,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } } { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -115,57 +105,44 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter supportVerticesOut[j] = vtx; } } - } } - -void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCapsuleShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //as an approximation, take the inertia of the box that bounds the spheres btTransform ident; ident.setIdentity(); - btScalar radius = getRadius(); - btVector3 halfExtents(radius,radius,radius); - halfExtents[getUpAxis()]+=getHalfHeight(); - - btScalar margin = CONVEX_DISTANCE_MARGIN; + btVector3 halfExtents(radius, radius, radius); + halfExtents[getUpAxis()] += getHalfHeight(); - btScalar lx=btScalar(2.)*(halfExtents[0]+margin); - btScalar ly=btScalar(2.)*(halfExtents[1]+margin); - btScalar lz=btScalar(2.)*(halfExtents[2]+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = btScalar(2.) * (halfExtents[0]); + btScalar ly = btScalar(2.) * (halfExtents[1]); + btScalar lz = btScalar(2.) * (halfExtents[2]); + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(.08333333); - inertia[0] = scaledmass * (y2+z2); - inertia[1] = scaledmass * (x2+z2); - inertia[2] = scaledmass * (x2+y2); - + inertia[0] = scaledmass * (y2 + z2); + inertia[1] = scaledmass * (x2 + z2); + inertia[2] = scaledmass * (x2 + y2); } -btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) +btCapsuleShapeX::btCapsuleShapeX(btScalar radius, btScalar height) { + m_collisionMargin = radius; m_upAxis = 0; - m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); + m_implicitShapeDimensions.setValue(0.5f * height, radius, radius); } - - - - - -btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) +btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius, btScalar height) { + m_collisionMargin = radius; m_upAxis = 2; - m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); + 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 f8c55ace4eb..138d0c0f7c1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -17,108 +17,96 @@ subject to the following restrictions: #define BT_CAPSULE_SHAPE_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///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. -ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btCapsuleShape : public btConvexInternalShape { protected: - int m_upAxis; + int m_upAxis; protected: ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. - btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; + btCapsuleShape() : btConvexInternalShape() { m_shapeType = CAPSULE_SHAPE_PROXYTYPE; }; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCapsuleShape(btScalar radius,btScalar height); + + btCapsuleShape(btScalar radius, btScalar height); ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - 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; - + //don't override the margin for capsules, their entire radius == margin + (void)collisionMargin; } - virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) 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(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - - aabbMin = center - extent; - aabbMax = center + extent; + btVector3 halfExtents(getRadius(), getRadius(), getRadius()); + halfExtents[m_upAxis] = getRadius() + getHalfHeight(); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + + aabbMin = center - extent; + aabbMax = center + extent; } - virtual const char* getName()const + virtual const char* getName() const { return "CapsuleShape"; } - int getUpAxis() const + int getUpAxis() const { return m_upAxis; } - btScalar getRadius() const + btScalar getRadius() const { - int radiusAxis = (m_upAxis+2)%3; + int radiusAxis = (m_upAxis + 2) % 3; return m_implicitShapeDimensions[radiusAxis]; } - btScalar getHalfHeight() const + btScalar getHalfHeight() const { return m_implicitShapeDimensions[m_upAxis]; } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; - + btVector3 unScaledImplicitShapeDimensions = m_implicitShapeDimensions / m_localScaling; btConvexInternalShape::setLocalScaling(scaling); - - m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; - + m_implicitShapeDimensions = (unScaledImplicitShapeDimensions * scaling); + //update m_collisionMargin, since entire radius==margin + int radiusAxis = (m_upAxis + 2) % 3; + m_collisionMargin = m_implicitShapeDimensions[radiusAxis]; } - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - btVector3 aniDir(0,0,0); - aniDir[getUpAxis()]=1; + btVector3 aniDir(0, 0, 0); + aniDir[getUpAxis()] = 1; return aniDir; } - - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData* dataBuffer); + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData * dataBuffer); }; ///btCapsuleShapeX represents a capsule around the Z axis @@ -126,17 +114,13 @@ public: class btCapsuleShapeX : public btCapsuleShape { public: + btCapsuleShapeX(btScalar radius, btScalar height); - btCapsuleShapeX(btScalar radius,btScalar height); - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "CapsuleX"; } - - - }; ///btCapsuleShapeZ represents a capsule around the Z axis @@ -144,45 +128,49 @@ public: class btCapsuleShapeZ : public btCapsuleShape { public: - btCapsuleShapeZ(btScalar radius,btScalar height); + btCapsuleShapeZ(btScalar radius, btScalar height); - //debugging - virtual const char* getName()const + //debugging + virtual const char* getName() const { return "CapsuleZ"; } - - }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCapsuleShapeData +struct btCapsuleShapeData { - btConvexInternalShapeData m_convexInternalShapeData; + btConvexInternalShapeData m_convexInternalShapeData; - int m_upAxis; + int m_upAxis; - char m_padding[4]; + char m_padding[4]; }; -SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const { return sizeof(btCapsuleShapeData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer; - - btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); + btCapsuleShapeData* shapeData = (btCapsuleShapeData*)dataBuffer; + + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); shapeData->m_upAxis = m_upAxis; - + + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; + return "btCapsuleShapeData"; } -SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer) +SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer) { m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions); m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin; @@ -191,4 +179,4 @@ SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* data m_upAxis = dataBuffer->m_upAxis; } -#endif //BT_CAPSULE_SHAPE_H +#endif //BT_CAPSULE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h index 474bf1fb499..abd8ab3eb5f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -19,9 +19,6 @@ subject to the following restrictions: ///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape. ///This collision margin is used by Gjk and some other algorithms ///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API -#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01) - - - -#endif //BT_COLLISION_MARGIN_H +#define CONVEX_DISTANCE_MARGIN btScalar(0.04) // btScalar(0.1)//;//btScalar(0.01) +#endif //BT_COLLISION_MARGIN_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 39ee21cad73..0b3640a65be 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -20,47 +20,44 @@ subject to the following restrictions: can be used by probes that are checking whether the library is actually installed. */ -extern "C" +extern "C" { -void btBulletCollisionProbe (); + void btBulletCollisionProbe(); -void btBulletCollisionProbe () {} + void btBulletCollisionProbe() {} } - - -void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const +void btCollisionShape::getBoundingSphere(btVector3& center, btScalar& radius) const { btTransform tr; tr.setIdentity(); - btVector3 aabbMin,aabbMax; + btVector3 aabbMin, aabbMax; - getAabb(tr,aabbMin,aabbMax); + getAabb(tr, aabbMin, aabbMax); - radius = (aabbMax-aabbMin).length()*btScalar(0.5); - center = (aabbMin+aabbMax)*btScalar(0.5); + radius = (aabbMax - aabbMin).length() * btScalar(0.5); + center = (aabbMin + aabbMax) * btScalar(0.5); } - -btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const +btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const { return getAngularMotionDisc() * defaultContactThreshold; } -btScalar btCollisionShape::getAngularMotionDisc() const +btScalar btCollisionShape::getAngularMotionDisc() const { ///@todo cache this value, to improve performance - btVector3 center; + btVector3 center; btScalar disc; - getBoundingSphere(center,disc); + getBoundingSphere(center, disc); disc += (center).length(); return disc; } -void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const +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); + getAabb(curTrans, temporalAabbMin, temporalAabbMax); btScalar temporalAabbMaxx = temporalAabbMax.getX(); btScalar temporalAabbMaxy = temporalAabbMax.getY(); @@ -70,50 +67,53 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b btScalar temporalAabbMinz = temporalAabbMin.getZ(); // add linear motion - btVector3 linMotion = linvel*timeStep; + btVector3 linMotion = linvel * timeStep; ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.x() > btScalar(0.)) - temporalAabbMaxx += linMotion.x(); + temporalAabbMaxx += linMotion.x(); else temporalAabbMinx += linMotion.x(); if (linMotion.y() > btScalar(0.)) - temporalAabbMaxy += linMotion.y(); + temporalAabbMaxy += linMotion.y(); else temporalAabbMiny += linMotion.y(); if (linMotion.z() > btScalar(0.)) - temporalAabbMaxz += linMotion.z(); + temporalAabbMaxz += linMotion.z(); else temporalAabbMinz += linMotion.z(); //add conservative angular motion btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; - btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); - temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); - temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + btVector3 angularMotion3d(angularMotion, angularMotion, angularMotion); + temporalAabbMin = btVector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); + temporalAabbMax = btVector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); temporalAabbMin -= angularMotion3d; temporalAabbMax += angularMotion3d; } ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer; - char* name = (char*) serializer->findNameForPointer(this); + btCollisionShapeData* shapeData = (btCollisionShapeData*)dataBuffer; + char* name = (char*)serializer->findNameForPointer(this); shapeData->m_name = (char*)serializer->getUniquePointer(name); if (shapeData->m_name) { serializer->serializeName(name); } shapeData->m_shapeType = m_shapeType; - //shapeData->m_padding//?? + + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding)); + return "btCollisionShapeData"; } -void btCollisionShape::serializeSingleShape(btSerializer* serializer) const +void btCollisionShape::serializeSingleShape(btSerializer* serializer) const { int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, (void*)this); }
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index 5e86568005e..c80e105a4d8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -19,12 +19,12 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types class btSerializer; - ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. -ATTRIBUTE_ALIGNED16(class) btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btCollisionShape { protected: int m_shapeType; @@ -32,10 +32,9 @@ protected: int m_userIndex; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) + btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) { } @@ -44,50 +43,47 @@ public: } ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void getBoundingSphere(btVector3& center,btScalar& radius) const; + virtual void getBoundingSphere(btVector3 & center, btScalar & radius) const; - ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. - virtual btScalar getAngularMotionDisc() const; - - virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const; + ///getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of-impact with rotations. + virtual btScalar getAngularMotionDisc() const; + virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const; ///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) const; - + void calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const; - - SIMD_FORCE_INLINE bool isPolyhedral() const + SIMD_FORCE_INLINE bool isPolyhedral() const { return btBroadphaseProxy::isPolyhedral(getShapeType()); } - SIMD_FORCE_INLINE bool isConvex2d() const + SIMD_FORCE_INLINE bool isConvex2d() const { return btBroadphaseProxy::isConvex2d(getShapeType()); } - SIMD_FORCE_INLINE bool isConvex() const + SIMD_FORCE_INLINE bool isConvex() const { return btBroadphaseProxy::isConvex(getShapeType()); } - SIMD_FORCE_INLINE bool isNonMoving() const + SIMD_FORCE_INLINE bool isNonMoving() const { return btBroadphaseProxy::isNonMoving(getShapeType()); } - SIMD_FORCE_INLINE bool isConcave() const + SIMD_FORCE_INLINE bool isConcave() const { return btBroadphaseProxy::isConcave(getShapeType()); } - SIMD_FORCE_INLINE bool isCompound() const + SIMD_FORCE_INLINE bool isCompound() const { return btBroadphaseProxy::isCompound(getShapeType()); } - SIMD_FORCE_INLINE bool isSoftBody() const + SIMD_FORCE_INLINE bool isSoftBody() const { return btBroadphaseProxy::isSoftBody(getShapeType()); } @@ -99,35 +95,35 @@ public: } #ifndef __SPU__ - virtual void setLocalScaling(const btVector3& scaling) =0; - virtual const btVector3& getLocalScaling() const =0; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0; - + virtual void setLocalScaling(const btVector3& scaling) = 0; + virtual const btVector3& getLocalScaling() const = 0; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const = 0; -//debugging support - virtual const char* getName()const =0 ; -#endif //__SPU__ + //debugging support + virtual const char* getName() const = 0; +#endif //__SPU__ - - int getShapeType() const { return m_shapeType; } + int getShapeType() const + { + return m_shapeType; + } ///the getAnisotropicRollingFrictionDirection can be used in combination with setAnisotropicFriction ///See Bullet/Demos/RollingFrictionDemo for an example - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - return btVector3(1,1,1); + return btVector3(1, 1, 1); } - virtual void setMargin(btScalar margin) = 0; - virtual btScalar getMargin() const = 0; + virtual void setMargin(btScalar margin) = 0; + virtual btScalar getMargin() const = 0; - ///optional user data pointer - void setUserPointer(void* userPtr) + void setUserPointer(void* userPtr) { m_userPointer = userPtr; } - void* getUserPointer() const + void* getUserPointer() const { return m_userPointer; } @@ -141,16 +137,16 @@ public: return m_userIndex; } - - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - virtual void serializeSingleShape(btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; -}; + virtual void serializeSingleShape(btSerializer * serializer) const; +}; +// clang-format off +// parser needs * with the name ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCollisionShapeData { @@ -158,13 +154,10 @@ struct btCollisionShapeData int m_shapeType; char m_padding[4]; }; - -SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const +// clang-format on +SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const { return sizeof(btCollisionShapeData); } - - -#endif //BT_COLLISION_SHAPE_H - +#endif //BT_COLLISION_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 1e7f36e0a17..fd7828b1047 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -19,26 +19,25 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity) -: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)), -m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)), -m_dynamicAabbTree(0), -m_updateRevision(1), -m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) + : m_localAabbMin(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)), + m_localAabbMax(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)), + m_dynamicAabbTree(0), + m_updateRevision(1), + m_collisionMargin(btScalar(0.)), + m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)) { m_shapeType = COMPOUND_SHAPE_PROXYTYPE; if (enableDynamicAabbTree) { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); } m_children.reserve(initialChildCapacity); } - btCompoundShape::~btCompoundShape() { if (m_dynamicAabbTree) @@ -48,7 +47,7 @@ btCompoundShape::~btCompoundShape() } } -void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) +void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape) { m_updateRevision++; //m_childTransforms.push_back(localTransform); @@ -60,11 +59,10 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio child.m_childShapeType = shape->getShapeType(); child.m_childMargin = shape->getMargin(); - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - shape->getAabb(localTransform,localAabbMin,localAabbMax); - for (int i=0;i<3;i++) + btVector3 localAabbMin, localAabbMax; + shape->getAabb(localTransform, localAabbMin, localAabbMax); + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) { @@ -74,31 +72,30 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio { m_localAabbMax[i] = localAabbMax[i]; } - } if (m_dynamicAabbTree) { - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index = m_children.size(); - child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) ); + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index)); } m_children.push_back(child); - } -void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb) +void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb) { m_children[childIndex].m_transform = newChildTransform; if (m_dynamicAabbTree) { ///update the dynamic aabb tree - btVector3 localAabbMin,localAabbMax; - m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax); - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + btVector3 localAabbMin, localAabbMax; + m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); //int index = m_children.size()-1; - m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); + m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds); } if (shouldRecalculateLocalAabb) @@ -110,35 +107,30 @@ void btCompoundShape::updateChildTransform(int childIndex, const btTransform& ne void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) { m_updateRevision++; - btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); + 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); - if (m_dynamicAabbTree) + m_children.swap(childShapeIndex, m_children.size() - 1); + if (m_dynamicAabbTree) m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex; m_children.pop_back(); - } - - void btCompoundShape::removeChildShape(btCollisionShape* shape) { m_updateRevision++; // 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--) + for (int i = m_children.size() - 1; i >= 0; i--) { - if(m_children[i].m_childShape == shape) + if (m_children[i].m_childShape == shape) { removeChildShapeByIndex(i); } } - - recalculateLocalAabb(); } @@ -147,15 +139,15 @@ void btCompoundShape::recalculateLocalAabb() // Recalculate the local aabb // Brute force, it iterates over all the shapes left. - m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); //extend the local aabbMin/aabbMax for (int j = 0; j < m_children.size(); j++) { - btVector3 localAabbMin,localAabbMax; + btVector3 localAabbMin, localAabbMax; m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) m_localAabbMin[i] = localAabbMin[i]; @@ -166,54 +158,48 @@ void btCompoundShape::recalculateLocalAabb() } ///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 +void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - + btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin); + btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin); + //avoid an illegal AABB when there are no children if (!m_children.size()) { - localHalfExtents.setValue(0,0,0); - localCenter.setValue(0,0,0); + localHalfExtents.setValue(0, 0, 0); + localCenter.setValue(0, 0, 0); } - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); - + localHalfExtents += btVector3(getMargin(), getMargin(), getMargin()); btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center-extent; - aabbMax = center+extent; - + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMin = center - extent; + aabbMax = center + extent; } -void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +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 aabbMin, aabbMax; + getAabb(ident, aabbMin, aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + 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()); - - inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); - inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); - inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz); + inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz); + inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly); } - - - -void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const +void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const { int n = m_children.size(); @@ -223,18 +209,18 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf for (k = 0; k < n; k++) { - btAssert(masses[k]>0); + btAssert(masses[k] > 0); center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; } - btAssert(totalMass>0); + btAssert(totalMass > 0); center /= totalMass; principal.setOrigin(center); btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); - for ( k = 0; k < n; k++) + for (k = 0; k < n; k++) { btVector3 i; m_children[k].m_childShape->calculateLocalInertia(masses[k], i); @@ -259,8 +245,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf 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[0] += o * -o.x(); + j[1] += o * -o.y(); j[2] += o * -o.z(); //add inertia tensor of pointmass @@ -273,59 +259,50 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); } - - - - void btCompoundShape::setLocalScaling(const btVector3& scaling) { - - for(int i = 0; i < m_children.size(); i++) + for (int i = 0; i < m_children.size(); i++) { btTransform childTrans = getChildTransform(i); btVector3 childScale = m_children[i].m_childShape->getLocalScaling(); -// childScale = childScale * (childTrans.getBasis() * scaling); + // childScale = childScale * (childTrans.getBasis() * scaling); childScale = childScale * scaling / m_localScaling; m_children[i].m_childShape->setLocalScaling(childScale); childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling); - updateChildTransform(i, childTrans,false); + updateChildTransform(i, childTrans, false); } - + m_localScaling = scaling; recalculateLocalAabb(); - } - void btCompoundShape::createAabbTreeFromChildren() { - if ( !m_dynamicAabbTree ) - { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + if (!m_dynamicAabbTree) + { + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); - for ( int index = 0; index < m_children.size(); index++ ) - { - btCompoundShapeChild &child = m_children[index]; + for (int index = 0; index < m_children.size(); index++) + { + btCompoundShapeChild& child = m_children[index]; - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax); + //extend the local aabbMin/aabbMax + btVector3 localAabbMin, localAabbMax; + child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax); - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index2 = index; - child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) ); - } - } + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2)); + } + } } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const { - - btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer; + btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer; btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); shapeData->m_collisionMargin = float(m_collisionMargin); @@ -333,27 +310,26 @@ const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serialize shapeData->m_childShapePtr = 0; if (shapeData->m_numChildShapes) { - btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes); + btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes); btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr; shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr); - for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++) + for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++) { memPtr->m_childMargin = float(m_children[i].m_childMargin); memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape); //don't serialize shapes that already have been serialized if (!serializer->findPointer(m_children[i].m_childShape)) { - btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1); - const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape); - } + btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1); + const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape); + } memPtr->m_childShapeType = m_children[i].m_childShapeType; m_children[i].m_transform.serializeFloat(memPtr->m_transform); } - serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr); } return "btCompoundShapeData"; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 4eef8dba306..7e2d0eb8170 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -27,45 +27,47 @@ subject to the following restrictions: //class btOptimizedBvh; struct btDbvt; -ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild +ATTRIBUTE_ALIGNED16(struct) +btCompoundShapeChild { BT_DECLARE_ALIGNED_ALLOCATOR(); - btTransform m_transform; - btCollisionShape* m_childShape; - int m_childShapeType; - btScalar m_childMargin; - struct btDbvtNode* m_node; + 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 ); + 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); } /// The btCompoundShape allows to store multiple other btCollisionShapes /// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape. -/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. +/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. /// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25 /// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape) -ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btCompoundShape : public btCollisionShape { protected: btAlignedObjectArray<btCompoundShapeChild> m_children; - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; - btDbvt* m_dynamicAabbTree; + btDbvt* m_dynamicAabbTree; ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated - int m_updateRevision; + int m_updateRevision; - btScalar m_collisionMargin; + btScalar m_collisionMargin; - btVector3 m_localScaling; + btVector3 m_localScaling; public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -74,17 +76,16 @@ public: virtual ~btCompoundShape(); - void addChildShape(const btTransform& localTransform,btCollisionShape* shape); + void addChildShape(const btTransform& localTransform, btCollisionShape* shape); /// Remove all children shapes that contain the specified shape - virtual void removeChildShape(btCollisionShape* shape); + virtual void removeChildShape(btCollisionShape * shape); void removeChildShapeByIndex(int childShapeindex); - - int getNumChildShapes() const + int getNumChildShapes() const { - return int (m_children.size()); + return int(m_children.size()); } btCollisionShape* getChildShape(int index) @@ -96,18 +97,17 @@ public: 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 + const btTransform& getChildTransform(int index) const { return m_children[index].m_transform; } ///set a new transform for a child, and update internal data structures (local aabb and dynamic tree) - void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true); - + void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true); btCompoundShapeChild* getChildList() { @@ -115,40 +115,40 @@ public: } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - virtual 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 recalculateLocalAabb(); - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } - virtual const char* getName()const + virtual const char* getName() const { return "Compound"; } - const btDbvt* getDynamicAabbTree() const + const btDbvt* getDynamicAabbTree() const { return m_dynamicAabbTree; } - - btDbvt* getDynamicAabbTree() + + btDbvt* getDynamicAabbTree() { return m_dynamicAabbTree; } @@ -160,21 +160,21 @@ public: ///"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; + void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const; - int getUpdateRevision() const + int getUpdateRevision() const { return m_updateRevision; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCompoundShapeChildData { @@ -197,16 +197,11 @@ struct btCompoundShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const { return sizeof(btCompoundShapeData); } - - - - - - -#endif //BT_COMPOUND_SHAPE_H +#endif //BT_COMPOUND_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp index 58ff84a5b02..5d396844dd8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -13,15 +13,12 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConcaveShape.h" btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.)) { - } btConcaveShape::~btConcaveShape() { - } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index 2917cc5b60f..716624e182b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -17,12 +17,13 @@ subject to the following restrictions: #define BT_CONCAVE_SHAPE_H #include "btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "btTriangleCallback.h" /// PHY_ScalarType enumerates possible scalar types. /// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use -typedef enum PHY_ScalarType { +typedef enum PHY_ScalarType +{ PHY_FLOAT, PHY_DOUBLE, PHY_INTEGER, @@ -33,30 +34,29 @@ typedef enum PHY_ScalarType { ///The btConcaveShape class provides an interface for non-moving (static) concave shapes. ///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape. -ATTRIBUTE_ALIGNED16(class) btConcaveShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btConcaveShape : public btCollisionShape { protected: btScalar m_collisionMargin; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btConcaveShape(); virtual ~btConcaveShape(); - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const = 0; - virtual btScalar getMargin() const { + virtual btScalar getMargin() const + { return m_collisionMargin; } virtual void setMargin(btScalar collisionMargin) { m_collisionMargin = collisionMargin; } - - - }; -#endif //BT_CONCAVE_SHAPE_H +#endif //BT_CONCAVE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 2d83c8bfbac..64a6f272ca5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -15,11 +15,9 @@ subject to the following restrictions: #include "btConeShape.h" - - -btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (), -m_radius (radius), -m_height(height) +btConeShape::btConeShape(btScalar radius, btScalar height) : btConvexInternalShape(), + m_radius(radius), + m_height(height) { m_shapeType = CONE_SHAPE_PROXYTYPE; setConeUpIndex(1); @@ -27,42 +25,40 @@ m_height(height) m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); } -btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height): -btConeShape(radius,height) +btConeShapeZ::btConeShapeZ(btScalar radius, btScalar height) : btConeShape(radius, height) { setConeUpIndex(2); } -btConeShapeX::btConeShapeX (btScalar radius,btScalar height): -btConeShape(radius,height) +btConeShapeX::btConeShapeX(btScalar radius, btScalar height) : btConeShape(radius, height) { setConeUpIndex(0); } ///choose upAxis index -void btConeShape::setConeUpIndex(int upIndex) +void btConeShape::setConeUpIndex(int upIndex) { switch (upIndex) { - case 0: + case 0: m_coneIndices[0] = 1; m_coneIndices[1] = 0; m_coneIndices[2] = 2; - break; - case 1: + break; + case 1: m_coneIndices[0] = 0; m_coneIndices[1] = 1; m_coneIndices[2] = 2; - break; - case 2: + break; + case 2: m_coneIndices[0] = 0; m_coneIndices[1] = 2; m_coneIndices[2] = 1; - break; - default: - btAssert(0); + break; + default: + btAssert(0); }; - + m_implicitShapeDimensions[m_coneIndices[0]] = m_radius; m_implicitShapeDimensions[m_coneIndices[1]] = m_height; m_implicitShapeDimensions[m_coneIndices[2]] = m_radius; @@ -70,72 +66,71 @@ void btConeShape::setConeUpIndex(int upIndex) btVector3 btConeShape::coneLocalSupport(const btVector3& v) const { - btScalar halfHeight = m_height * btScalar(0.5); - if (v[m_coneIndices[1]] > v.length() * m_sinAngle) - { - btVector3 tmp; - - tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = halfHeight; - tmp[m_coneIndices[2]] = btScalar(0.); - return tmp; - } - else { - btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); - if (s > SIMD_EPSILON) { - btScalar d = m_radius / s; - btVector3 tmp; - tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; - tmp[m_coneIndices[1]] = -halfHeight; - tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; - return tmp; - } - else { + if (v[m_coneIndices[1]] > v.length() * m_sinAngle) + { btVector3 tmp; + tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[1]] = halfHeight; tmp[m_coneIndices[2]] = btScalar(0.); return tmp; } - } - + else + { + btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); + if (s > SIMD_EPSILON) + { + btScalar d = m_radius / s; + btVector3 tmp; + tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; + return tmp; + } + else + { + btVector3 tmp; + tmp[m_coneIndices[0]] = btScalar(0.); + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = btScalar(0.); + return tmp; + } + } } -btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const +btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return coneLocalSupport(vec); + return coneLocalSupport(vec); } -void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { const btVector3& vec = vectors[i]; supportVerticesOut[i] = coneLocalSupport(vec); } } - -btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const +btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = coneLocalSupport(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - -void btConeShape::setLocalScaling(const btVector3& scaling) +void btConeShape::setLocalScaling(const btVector3& scaling) { int axis = m_coneIndices[1]; int r1 = m_coneIndices[0]; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 4a0df0d558b..ee6786c8074 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -17,155 +17,157 @@ subject to the following restrictions: #define BT_CONE_MINKOWSKI_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///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. -ATTRIBUTE_ALIGNED16(class) btConeShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btConeShape : public btConvexInternalShape { - btScalar m_sinAngle; btScalar m_radius; btScalar m_height; - int m_coneIndices[3]; + int m_coneIndices[3]; btVector3 coneLocalSupport(const btVector3& v) const; - public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConeShape (btScalar radius,btScalar height); - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - btScalar getRadius() const { return m_radius;} - btScalar getHeight() const { return m_height;} + btConeShape(btScalar radius, btScalar height); + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + btScalar getRadius() const { return m_radius; } + btScalar getHeight() const { return m_height; } + + void setRadius(const btScalar radius) + { + m_radius = radius; + } + void setHeight(const btScalar height) + { + m_height = height; + } + + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const { btTransform identity; identity.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(identity,aabbMin,aabbMax); - - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + btVector3 aabbMin, aabbMax; - btScalar margin = getMargin(); + getAabb(identity, aabbMin, aabbMax); // This already contains the margin + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); - btScalar lx=btScalar(2.)*(halfExtents.x()+margin); - btScalar ly=btScalar(2.)*(halfExtents.y()+margin); - btScalar lz=btScalar(2.)*(halfExtents.z()+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + 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)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); + + // inertia.x() = scaledmass * (y2+z2); + // inertia.y() = scaledmass * (x2+z2); + // inertia.z() = scaledmass * (x2+y2); + } -// inertia.x() = scaledmass * (y2+z2); -// inertia.y() = scaledmass * (x2+z2); -// inertia.z() = scaledmass * (x2+y2); + virtual const char* getName() const + { + return "Cone"; } + ///choose upAxis index + void setConeUpIndex(int upIndex); - virtual const char* getName()const - { - return "Cone"; - } - - ///choose upAxis index - void setConeUpIndex(int upIndex); - - int getConeUpIndex() const - { - return m_coneIndices[1]; - } - - virtual btVector3 getAnisotropicRollingFrictionDirection() const + int getConeUpIndex() const { - return btVector3 (0,1,0); + return m_coneIndices[1]; } - virtual void setLocalScaling(const btVector3& scaling); - - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual btVector3 getAnisotropicRollingFrictionDirection() const + { + return btVector3(0, 1, 0); + } + + virtual void setLocalScaling(const btVector3& scaling); + + virtual int calculateSerializeBufferSize() const; + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///btConeShape implements a Cone shape, around the X axis class btConeShapeX : public btConeShape { - public: - btConeShapeX(btScalar radius,btScalar height); +public: + btConeShapeX(btScalar radius, btScalar height); - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - return btVector3 (1,0,0); + return btVector3(1, 0, 0); } //debugging - virtual const char* getName()const + virtual const char* getName() const { return "ConeX"; } - - }; ///btConeShapeZ implements a Cone shape, around the Z axis class btConeShapeZ : public btConeShape { public: - btConeShapeZ(btScalar radius,btScalar height); + btConeShapeZ(btScalar radius, btScalar height); - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - return btVector3 (0,0,1); + return btVector3(0, 0, 1); } //debugging - virtual const char* getName()const + virtual const char* getName() const { return "ConeZ"; } - - }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConeShapeData +struct btConeShapeData { - btConvexInternalShapeData m_convexInternalShapeData; - - int m_upIndex; - - char m_padding[4]; + btConvexInternalShapeData m_convexInternalShapeData; + + int m_upIndex; + + char m_padding[4]; }; -SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const { return sizeof(btConeShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btConeShapeData* shapeData = (btConeShapeData*) dataBuffer; - - btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); - + btConeShapeData* shapeData = (btConeShapeData*)dataBuffer; + + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); + shapeData->m_upIndex = m_coneIndices[1]; - + + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; + return "btConeShapeData"; } -#endif //BT_CONE_MINKOWSKI_H - +#endif //BT_CONE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp index 10ea3e981af..7d3d1d362fb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp @@ -15,54 +15,48 @@ subject to the following restrictions: #include "btConvex2dShape.h" -btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape): -btConvexShape (), m_childConvexShape(convexChildShape) +btConvex2dShape::btConvex2dShape(btConvexShape* convexChildShape) : btConvexShape(), m_childConvexShape(convexChildShape) { m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE; } - + btConvex2dShape::~btConvex2dShape() { } - - -btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); } -void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors); } - -btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec) const { return m_childConvexShape->localGetSupportingVertex(vec); } - -void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btConvex2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { ///this linear upscaling is not realistic, but we don't deal with large mass ratios... - m_childConvexShape->calculateLocalInertia(mass,inertia); + m_childConvexShape->calculateLocalInertia(mass, inertia); } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btConvex2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - m_childConvexShape->getAabb(t,aabbMin,aabbMax); + m_childConvexShape->getAabb(t, aabbMin, aabbMax); } -void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btConvex2dShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); + m_childConvexShape->getAabbSlow(t, aabbMin, aabbMax); } -void btConvex2dShape::setLocalScaling(const btVector3& scaling) +void btConvex2dShape::setLocalScaling(const btVector3& scaling) { m_childConvexShape->setLocalScaling(scaling); } @@ -72,21 +66,21 @@ const btVector3& btConvex2dShape::getLocalScaling() const return m_childConvexShape->getLocalScaling(); } -void btConvex2dShape::setMargin(btScalar margin) +void btConvex2dShape::setMargin(btScalar margin) { m_childConvexShape->setMargin(margin); } -btScalar btConvex2dShape::getMargin() const +btScalar btConvex2dShape::getMargin() const { return m_childConvexShape->getMargin(); } -int btConvex2dShape::getNumPreferredPenetrationDirections() const +int btConvex2dShape::getNumPreferredPenetrationDirections() const { return m_childConvexShape->getNumPreferredPenetrationDirections(); } - -void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + +void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); + m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h index bbd1caf42fb..cd4f1ef7b89 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h @@ -17,66 +17,61 @@ subject to the following restrictions: #define BT_CONVEX_2D_SHAPE_H #include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btConvex2dShape allows to use arbitrary convex shapes as 2d convex shapes, with the Z component assumed to be 0. ///For 2d boxes, the btBox2dShape is recommended. -ATTRIBUTE_ALIGNED16(class) btConvex2dShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btConvex2dShape : public btConvexShape { - btConvexShape* m_childConvexShape; + btConvexShape* m_childConvexShape; - public: - +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvex2dShape( btConvexShape* convexChildShape); - + + btConvex2dShape(btConvexShape * convexChildShape); + virtual ~btConvex2dShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + 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 batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - btConvexShape* getChildShape() + btConvexShape* getChildShape() { return m_childConvexShape; } - const btConvexShape* getChildShape() const + const btConvexShape* getChildShape() const { return m_childConvexShape; } - virtual const char* getName()const + virtual const char* getName() const { return "Convex2dShape"; } - - /////////////////////////// - ///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; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling) ; - virtual const btVector3& getLocalScaling() const ; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setMargin(btScalar margin); - virtual btScalar getMargin() const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; - virtual int getNumPreferredPenetrationDirections() const; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + virtual int getNumPreferredPenetrationDirections() const; + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; }; -#endif //BT_CONVEX_2D_SHAPE_H +#endif //BT_CONVEX_2D_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 02ea5033b15..703de45922e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -13,24 +13,26 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" #include "LinearMath/btSerializer.h" +#include "btConvexPolyhedron.h" +#include "LinearMath/btConvexHullComputer.h" -btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape () +btConvexHullShape ::btConvexHullShape(const btScalar* points, int numPoints, int stride) : btPolyhedralConvexAabbCachingShape() { m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE; m_unscaledPoints.resize(numPoints); unsigned char* pointsAddress = (unsigned char*)points; - for (int i=0;i<numPoints;i++) + for (int i = 0; i < numPoints; i++) { btScalar* point = (btScalar*)pointsAddress; m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]); @@ -38,11 +40,8 @@ btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int } recalcLocalAabb(); - } - - void btConvexHullShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; @@ -54,83 +53,81 @@ void btConvexHullShape::addPoint(const btVector3& point, bool recalculateLocalAa m_unscaledPoints.push_back(point); if (recalculateLocalAabb) recalcLocalAabb(); - } -btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btScalar maxDot = btScalar(-BT_LARGE_FLOAT); - // Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically. - if( 0 < m_unscaledPoints.size() ) - { - btVector3 scaled = vec * m_localScaling; - int index = (int) scaled.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints - return m_unscaledPoints[index] * m_localScaling; - } + // Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically. + if (0 < m_unscaledPoints.size()) + { + btVector3 scaled = vec * m_localScaling; + int index = (int)scaled.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints + return m_unscaledPoints[index] * m_localScaling; + } - return supVec; + return supVec; } -void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { btScalar newDot; //use 'w' component of supportVerticesOut? { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } } - for (int j=0;j<numVectors;j++) - { - btVector3 vec = vectors[j] * m_localScaling; // dot(a*b,c) = dot(a,b*c) - if( 0 < m_unscaledPoints.size() ) - { - int i = (int) vec.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), newDot); - supportVerticesOut[j] = getScaledPoint(i); - supportVerticesOut[j][3] = newDot; - } - else - supportVerticesOut[j][3] = -BT_LARGE_FLOAT; - } - - - + for (int j = 0; j < numVectors; j++) + { + btVector3 vec = vectors[j] * m_localScaling; // dot(a*b,c) = dot(a,b*c) + if (0 < m_unscaledPoints.size()) + { + int i = (int)vec.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), newDot); + supportVerticesOut[j] = getScaledPoint(i); + supportVerticesOut[j][3] = newDot; + } + else + supportVerticesOut[j][3] = -BT_LARGE_FLOAT; + } } - - -btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - - - - - - - +void btConvexHullShape::optimizeConvexHull() +{ + btConvexHullComputer conv; + conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3), m_unscaledPoints.size(), 0.f, 0.f); + int numVerts = conv.vertices.size(); + m_unscaledPoints.resize(0); + for (int i = 0; i < numVerts; i++) + { + m_unscaledPoints.push_back(conv.vertices[i]); + } +} //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo -int btConvexHullShape::getNumVertices() const +int btConvexHullShape::getNumVertices() const { return m_unscaledPoints.size(); } @@ -140,111 +137,108 @@ int btConvexHullShape::getNumEdges() const return m_unscaledPoints.size(); } -void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const +void btConvexHullShape::getEdge(int i, btVector3& pa, btVector3& pb) const { - - int index0 = i%m_unscaledPoints.size(); - int index1 = (i+1)%m_unscaledPoints.size(); + int index0 = i % m_unscaledPoints.size(); + int index1 = (i + 1) % m_unscaledPoints.size(); pa = getScaledPoint(index0); pb = getScaledPoint(index1); } -void btConvexHullShape::getVertex(int i,btVector3& vtx) const +void btConvexHullShape::getVertex(int i, btVector3& vtx) const { vtx = getScaledPoint(i); } -int btConvexHullShape::getNumPlanes() const +int btConvexHullShape::getNumPlanes() const { return 0; } -void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexHullShape::getPlane(btVector3&, btVector3&, int) const { - btAssert(0); } //not yet -bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const +bool btConvexHullShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const { //int szc = sizeof(btConvexHullShapeData); - btConvexHullShapeData* shapeData = (btConvexHullShapeData*) dataBuffer; + btConvexHullShapeData* shapeData = (btConvexHullShapeData*)dataBuffer; btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); int numElem = m_unscaledPoints.size(); shapeData->m_numUnscaledPoints = numElem; #ifdef BT_USE_DOUBLE_PRECISION shapeData->m_unscaledPointsFloatPtr = 0; - shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; + shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0; #else - shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; + shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0; shapeData->m_unscaledPointsDoublePtr = 0; #endif - + if (numElem) { int sz = sizeof(btVector3Data); - // int sz2 = sizeof(btVector3DoubleData); - // int sz3 = sizeof(btVector3FloatData); - btChunk* chunk = serializer->allocate(sz,numElem); + // int sz2 = sizeof(btVector3DoubleData); + // int sz3 = sizeof(btVector3FloatData); + btChunk* chunk = serializer->allocate(sz, numElem); btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_unscaledPoints[i].serialize(*memPtr); } - serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]); + serializer->finalizeChunk(chunk, btVector3DataName, BT_ARRAY_CODE, (void*)&m_unscaledPoints[0]); } - + + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding3, 0, sizeof(shapeData->m_padding3)); + return "btConvexHullShapeData"; } -void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const { #if 1 minProj = FLT_MAX; maxProj = -FLT_MAX; int numVerts = m_unscaledPoints.size(); - for(int i=0;i<numVerts;i++) + for (int i = 0; i < numVerts; i++) { btVector3 vtx = m_unscaledPoints[i] * m_localScaling; btVector3 pt = trans * vtx; btScalar dp = pt.dot(dir); - if(dp < minProj) + if (dp < minProj) { minProj = dp; witnesPtMin = pt; } - if(dp > maxProj) + if (dp > maxProj) { maxProj = dp; - witnesPtMax=pt; + witnesPtMax = pt; } } #else - btVector3 localAxis = dir*trans.getBasis(); - witnesPtMin = trans(localGetSupportingVertex(localAxis)); + btVector3 localAxis = dir * trans.getBasis(); + witnesPtMin = trans(localGetSupportingVertex(localAxis)); witnesPtMax = trans(localGetSupportingVertex(-localAxis)); minProj = witnesPtMin.dot(dir); maxProj = witnesPtMax.dot(dir); #endif - if(minProj>maxProj) + if (minProj > maxProj) { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); + btSwap(minProj, maxProj); + btSwap(witnesPtMin, witnesPtMax); } - - } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 3bd598ec4e9..96136d7dd72 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -17,28 +17,26 @@ subject to the following restrictions: #define BT_CONVEX_HULL_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" - ///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 btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexHullShape : public btPolyhedralConvexAabbCachingShape { - btAlignedObjectArray<btVector3> m_unscaledPoints; + btAlignedObjectArray<btVector3> m_unscaledPoints; 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. ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. ///btConvexHullShape make an internal copy of the points. - btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3)); + btConvexHullShape(const btScalar* points = 0, int numPoints = 0, int stride = sizeof(btVector3)); void addPoint(const btVector3& point, bool recalculateLocalAabb = true); - btVector3* getUnscaledPoints() { return &m_unscaledPoints[0]; @@ -55,49 +53,46 @@ public: return getUnscaledPoints(); } - - + void optimizeConvexHull(); - SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const + SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const { return m_unscaledPoints[i] * m_localScaling; } - SIMD_FORCE_INLINE int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { return m_unscaledPoints.size(); } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; //debugging - virtual const char* getName()const {return "Convex";} + virtual const char* getName() const { return "Convex"; } - - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btConvexHullShapeData { @@ -111,12 +106,11 @@ struct btConvexHullShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const { return sizeof(btConvexHullShapeData); } - -#endif //BT_CONVEX_HULL_SHAPE_H - +#endif //BT_CONVEX_HULL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp index 083d60b1b1e..b847f8f40f6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -13,139 +13,125 @@ subject to the following restrictions: 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) + : m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)), + m_collisionMargin(CONVEX_DISTANCE_MARGIN) { } - -void btConvexInternalShape::setLocalScaling(const btVector3& scaling) +void btConvexInternalShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling.absolute(); } - - -void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const +void btConvexInternalShape::getAabbSlow(const btTransform& trans, btVector3& minAabb, btVector3& maxAabb) const { #ifndef __SPU__ //use localGetSupportingVertexWithoutMargin? btScalar margin = getMargin(); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); - btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); + btVector3 sv = localGetSupportingVertex(vec * trans.getBasis()); btVector3 tmp = trans(sv); - maxAabb[i] = tmp[i]+margin; + maxAabb[i] = tmp[i] + margin; vec[i] = btScalar(-1.); - tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); - minAabb[i] = tmp[i]-margin; + tmp = trans(localGetSupportingVertex(vec * trans.getBasis())); + minAabb[i] = tmp[i] - margin; } #endif } - - -btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec) const { #ifndef __SPU__ - btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; #else btAssert(0); - return btVector3(0,0,0); -#endif //__SPU__ - - } - + return btVector3(0, 0, 0); +#endif //__SPU__ +} btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape() - : btConvexInternalShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) + : btConvexInternalShape(), + m_localAabbMin(1, 1, 1), + m_localAabbMax(-1, -1, -1), + m_isLocalAabbValid(false) { } - -void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); } -void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) +void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) { btConvexInternalShape::setLocalScaling(scaling); recalcLocalAabb(); } - -void btConvexInternalAabbCachingShape::recalcLocalAabb() +void btConvexInternalAabbCachingShape::recalcLocalAabb() { m_isLocalAabbValid = true; - - #if 1 + +#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(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.) - }; - + { + 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 ) + + for (int i = 0; i < 3; ++i) { - m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; - m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; + m_localAabbMax[i] = _supporting[i][i]; + m_localAabbMin[i] = _supporting[i + 3][i]; } - - #else - for (int i=0;i<3;i++) +#else + + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i]; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i]; } - #endif +#endif } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index 37e04f5fc81..a28c57de4b8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -19,39 +19,35 @@ subject to the following restrictions: #include "btConvexShape.h" #include "LinearMath/btAabbUtil2.h" - ///The btConvexInternalShape is an internal base class, shared by most convex shape implementations. ///The btConvexInternalShape uses a default collision margin set to CONVEX_DISTANCE_MARGIN. ///This collision margin used by Gjk and some other algorithms, see also btCollisionMargin.h -///Note that when creating small shapes (derived from btConvexInternalShape), +///Note that when creating small shapes (derived from btConvexInternalShape), ///you need to make sure to set a smaller collision margin, using the 'setMargin' API ///There is a automatic mechanism 'setSafeMargin' used by btBoxShape and btCylinderShape -ATTRIBUTE_ALIGNED16(class) btConvexInternalShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btConvexInternalShape : public btConvexShape { - - protected: - +protected: //local scaling. collisionMargin is not scaled ! - btVector3 m_localScaling; + btVector3 m_localScaling; + + btVector3 m_implicitShapeDimensions; - btVector3 m_implicitShapeDimensions; - - btScalar m_collisionMargin; + btScalar m_collisionMargin; - btScalar m_padding; + btScalar m_padding; btConvexInternalShape(); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); virtual ~btConvexInternalShape() { - } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; const btVector3& getImplicitShapeDimensions() const { @@ -62,163 +58,151 @@ public: ///changing a collision shape while the body is in the world is not recommended, ///it is best to remove the body from the world, then make the change, and re-add it ///alternatively flush the contact points, see documentation for 'cleanProxyFromPairs' - void setImplicitShapeDimensions(const btVector3& dimensions) + void setImplicitShapeDimensions(const btVector3& dimensions) { m_implicitShapeDimensions = dimensions; } - void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) + void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) { - btScalar safeMargin = defaultMarginMultiplier*minDimension; + btScalar safeMargin = defaultMarginMultiplier * minDimension; if (safeMargin < getMargin()) { setMargin(safeMargin); } } - void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f) + void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f) { //see http://code.google.com/p/bullet/issues/detail?id=349 //this margin check could could be added to other collision shapes too, //or add some assert/warning somewhere - btScalar minDimension=halfExtents[halfExtents.minAxis()]; + btScalar minDimension = halfExtents[halfExtents.minAxis()]; setSafeMargin(minDimension, defaultMarginMultiplier); } ///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 + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - getAabbSlow(t,aabbMin,aabbMax); + getAabbSlow(t, aabbMin, aabbMax); } + virtual void getAabbSlow(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 setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - const btVector3& getLocalScalingNV() const + const btVector3& getLocalScalingNV() const { return m_localScaling; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } - btScalar getMarginNV() const + btScalar getMarginNV() const { return m_collisionMargin; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 0; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { (void)penetrationVector; (void)index; btAssert(0); } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConvexInternalShapeData +struct btConvexInternalShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; - btVector3FloatData m_implicitShapeDimensions; - - float m_collisionMargin; + btVector3FloatData m_implicitShapeDimensions; - int m_padding; + float m_collisionMargin; + int m_padding; }; - - -SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const { return sizeof(btConvexInternalShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer; + btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*)dataBuffer; btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions); m_localScaling.serializeFloat(shapeData->m_localScaling); shapeData->m_collisionMargin = float(m_collisionMargin); + // Fill padding with zeros to appease msan. + shapeData->m_padding = 0; + return "btConvexInternalShapeData"; } - - - ///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations class btConvexInternalAabbCachingShape : public btConvexInternalShape { - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - bool m_isLocalAabbValid; - + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + protected: - btConvexInternalAabbCachingShape(); - - void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) + + void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax) { m_isLocalAabbValid = true; m_localAabbMin = aabbMin; m_localAabbMax = aabbMax; } - inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const + inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const { btAssert(m_isLocalAabbValid); aabbMin = m_localAabbMin; aabbMax = m_localAabbMax; } - inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const + inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const { - //lazy evaluation of local aabb btAssert(m_isLocalAabbValid); - btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); + btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax); } - -public: - - virtual void setLocalScaling(const btVector3& scaling); - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; +public: + virtual void setLocalScaling(const btVector3& scaling); - void recalcLocalAabb(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + void recalcLocalAabb(); }; -#endif //BT_CONVEX_INTERNAL_SHAPE_H +#endif //BT_CONVEX_INTERNAL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp index ad1d1bf78f8..f00a440fa35 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -25,81 +25,73 @@ void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) } #ifndef __SPU__ -btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btScalar maxDot = btScalar(-BT_LARGE_FLOAT); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } - - if( m_numPoints > 0 ) - { - // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically. - // btVector3 scaled = vec * m_localScaling; - int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints - return getScaledPoint(index); - } + + if (m_numPoints > 0) + { + // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically. + // btVector3 scaled = vec * m_localScaling; + int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints + return getScaledPoint(index); + } return supVec; } -void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for( int j = 0; j < numVectors; j++ ) - { - const btVector3& vec = vectors[j] * m_localScaling; // dot( a*c, b) = dot(a, b*c) - btScalar maxDot; - int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); - supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT); - if( 0 <= index ) - { - //WARNING: don't swap next lines, the w component would get overwritten! - supportVerticesOut[j] = getScaledPoint(index); - supportVerticesOut[j][3] = maxDot; - } - } - + for (int j = 0; j < numVectors; j++) + { + const btVector3& vec = vectors[j] * m_localScaling; // dot( a*c, b) = dot(a, b*c) + btScalar maxDot; + int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot); + supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT); + if (0 <= index) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = getScaledPoint(index); + supportVerticesOut[j][3] = maxDot; + } + } } - - -btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - #endif - - - - - //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo -int btConvexPointCloudShape::getNumVertices() const +int btConvexPointCloudShape::getNumVertices() const { return m_numPoints; } @@ -109,31 +101,29 @@ int btConvexPointCloudShape::getNumEdges() const return 0; } -void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const +void btConvexPointCloudShape::getEdge(int i, btVector3& pa, btVector3& pb) const { - btAssert (0); + btAssert(0); } -void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const +void btConvexPointCloudShape::getVertex(int i, btVector3& vtx) const { - vtx = m_unscaledPoints[i]*m_localScaling; + vtx = m_unscaledPoints[i] * m_localScaling; } -int btConvexPointCloudShape::getNumPlanes() const +int btConvexPointCloudShape::getNumPlanes() const { return 0; } -void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexPointCloudShape::getPlane(btVector3&, btVector3&, int) const { - btAssert(0); } //not yet -bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const +bool btConvexPointCloudShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h index 54b5afac3ec..c7d554a4d3d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -17,11 +17,12 @@ subject to the following restrictions: #define BT_CONVEX_POINT_CLOUD_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" ///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. -ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape { btVector3* m_unscaledPoints; int m_numPoints; @@ -31,13 +32,13 @@ public: btConvexPointCloudShape() { - m_localScaling.setValue(1.f,1.f,1.f); + m_localScaling.setValue(1.f, 1.f, 1.f); m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; m_unscaledPoints = 0; m_numPoints = 0; } - btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) + btConvexPointCloudShape(btVector3 * points, int numPoints, const btVector3& localScaling, bool computeAabb = true) { m_localScaling = localScaling; m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; @@ -48,7 +49,7 @@ public: recalcLocalAabb(); } - void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f)) + void setPoints(btVector3 * points, int numPoints, bool computeAabb = true, const btVector3& localScaling = btVector3(1.f, 1.f, 1.f)) { m_unscaledPoints = points; m_numPoints = numPoints; @@ -58,48 +59,45 @@ public: recalcLocalAabb(); } - SIMD_FORCE_INLINE btVector3* getUnscaledPoints() + SIMD_FORCE_INLINE btVector3* getUnscaledPoints() { return m_unscaledPoints; } - SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const + SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const { return m_unscaledPoints; } - SIMD_FORCE_INLINE int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { return m_numPoints; } - SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const + SIMD_FORCE_INLINE btVector3 getScaledPoint(int index) const { return m_unscaledPoints[index] * m_localScaling; } #ifndef __SPU__ - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; #endif - //debugging - virtual const char* getName()const {return "ConvexPointCloud";} + virtual const char* getName() const { return "ConvexPointCloud"; } - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); }; - -#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H - +#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 4f45319a83a..9694f4ddb3d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans ///Separating axis rest based on work from Pierre Terdiman, see ///And contact clipping based on work from Simon Hobbs @@ -21,49 +20,45 @@ subject to the following restrictions: #include "btConvexPolyhedron.h" #include "LinearMath/btHashMap.h" - btConvexPolyhedron::btConvexPolyhedron() { - } btConvexPolyhedron::~btConvexPolyhedron() { - } - -inline bool IsAlmostZero(const btVector3& v) +inline bool IsAlmostZero1(const btVector3& v) { - if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false; + if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; } struct btInternalVertexPair { - btInternalVertexPair(short int v0,short int v1) - :m_v0(v0), - m_v1(v1) + btInternalVertexPair(short int v0, short int v1) + : m_v0(v0), + m_v1(v1) { - if (m_v1>m_v0) - btSwap(m_v0,m_v1); + if (m_v1 > m_v0) + btSwap(m_v0, m_v1); } short int m_v0; short int m_v1; int getHash() const { - return m_v0+(m_v1<<16); + return m_v0 + (m_v1 << 16); } bool equals(const btInternalVertexPair& other) const { - return m_v0==other.m_v0 && m_v1==other.m_v1; + return m_v0 == other.m_v0 && m_v1 == other.m_v1; } }; struct btInternalEdge { btInternalEdge() - :m_face0(-1), - m_face1(-1) + : m_face0(-1), + m_face1(-1) { } short int m_face0; @@ -75,23 +70,31 @@ struct btInternalEdge #ifdef TEST_INTERNAL_OBJECTS bool btConvexPolyhedron::testContainment() const { - for(int p=0;p<8;p++) + for (int p = 0; p < 8; p++) { btVector3 LocalPt; - if(p==0) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]); - else if(p==1) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]); - else if(p==2) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]); - else if(p==3) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]); - else if(p==4) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]); - else if(p==5) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]); - else if(p==6) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]); - else if(p==7) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]); - - for(int i=0;i<m_faces.size();i++) + if (p == 0) + LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]); + else if (p == 1) + LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]); + else if (p == 2) + LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]); + else if (p == 3) + LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]); + else if (p == 4) + LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]); + else if (p == 5) + LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]); + else if (p == 6) + LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]); + else if (p == 7) + LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]); + + for (int i = 0; i < m_faces.size(); i++) { const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3]; - if(d>0.0f) + if (d > 0.0f) return false; } } @@ -99,33 +102,28 @@ bool btConvexPolyhedron::testContainment() const } #endif -void btConvexPolyhedron::initialize() +void btConvexPolyhedron::initialize() { + btHashMap<btInternalVertexPair, btInternalEdge> edges; - btHashMap<btInternalVertexPair,btInternalEdge> edges; - - btScalar TotalArea = 0.0f; - - m_localCenter.setValue(0, 0, 0); - for(int i=0;i<m_faces.size();i++) + for (int i = 0; i < m_faces.size(); i++) { int numVertices = m_faces[i].m_indices.size(); int NbTris = numVertices; - for(int j=0;j<NbTris;j++) + for (int j = 0; j < NbTris; j++) { - int k = (j+1)%numVertices; - btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + int k = (j + 1) % numVertices; + btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]); btInternalEdge* edptr = edges.find(vp); - btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0]; + btVector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0]; edge.normalize(); bool found = false; - for (int p=0;p<m_uniqueEdges.size();p++) + for (int p = 0; p < m_uniqueEdges.size(); p++) { - - if (IsAlmostZero(m_uniqueEdges[p]-edge) || - IsAlmostZero(m_uniqueEdges[p]+edge)) + if (IsAlmostZero1(m_uniqueEdges[p] - edge) || + IsAlmostZero1(m_uniqueEdges[p] + edge)) { found = true; break; @@ -139,105 +137,107 @@ void btConvexPolyhedron::initialize() if (edptr) { - btAssert(edptr->m_face0>=0); - btAssert(edptr->m_face1<0); + btAssert(edptr->m_face0 >= 0); + btAssert(edptr->m_face1 < 0); edptr->m_face1 = i; - } else + } + else { btInternalEdge ed; ed.m_face0 = i; - edges.insert(vp,ed); + edges.insert(vp, ed); } } } #ifdef USE_CONNECTED_FACES - for(int i=0;i<m_faces.size();i++) + for (int i = 0; i < m_faces.size(); i++) { int numVertices = m_faces[i].m_indices.size(); m_faces[i].m_connectedFaces.resize(numVertices); - for(int j=0;j<numVertices;j++) + for (int j = 0; j < numVertices; j++) { - int k = (j+1)%numVertices; - btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + int k = (j + 1) % numVertices; + btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]); btInternalEdge* edptr = edges.find(vp); btAssert(edptr); - btAssert(edptr->m_face0>=0); - btAssert(edptr->m_face1>=0); + btAssert(edptr->m_face0 >= 0); + btAssert(edptr->m_face1 >= 0); - int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; + int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0; m_faces[i].m_connectedFaces[j] = connectedFace; } } -#endif//USE_CONNECTED_FACES +#endif //USE_CONNECTED_FACES + + initialize2(); +} - for(int i=0;i<m_faces.size();i++) +void btConvexPolyhedron::initialize2() +{ + m_localCenter.setValue(0, 0, 0); + btScalar TotalArea = 0.0f; + for (int i = 0; i < m_faces.size(); i++) { int numVertices = m_faces[i].m_indices.size(); - int NbTris = numVertices-2; - + int NbTris = numVertices - 2; + const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]]; - for(int j=1;j<=NbTris;j++) + for (int j = 1; j <= NbTris; j++) { - int k = (j+1)%numVertices; + int k = (j + 1) % numVertices; const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]]; const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]]; btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f; - btVector3 Center = (p0+p1+p2)/3.0f; + btVector3 Center = (p0 + p1 + p2) / 3.0f; m_localCenter += Area * Center; TotalArea += Area; } } m_localCenter /= TotalArea; - - - #ifdef TEST_INTERNAL_OBJECTS - if(1) + if (1) { m_radius = FLT_MAX; - for(int i=0;i<m_faces.size();i++) + for (int i = 0; i < m_faces.size(); i++) { const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]); - if(dist<m_radius) + if (dist < m_radius) m_radius = dist; } - btScalar MinX = FLT_MAX; btScalar MinY = FLT_MAX; btScalar MinZ = FLT_MAX; btScalar MaxX = -FLT_MAX; btScalar MaxY = -FLT_MAX; btScalar MaxZ = -FLT_MAX; - for(int i=0; i<m_vertices.size(); i++) + for (int i = 0; i < m_vertices.size(); i++) { const btVector3& pt = m_vertices[i]; - if(pt.x()<MinX) MinX = pt.x(); - if(pt.x()>MaxX) MaxX = pt.x(); - if(pt.y()<MinY) MinY = pt.y(); - if(pt.y()>MaxY) MaxY = pt.y(); - if(pt.z()<MinZ) MinZ = pt.z(); - if(pt.z()>MaxZ) MaxZ = pt.z(); + if (pt.x() < MinX) MinX = pt.x(); + if (pt.x() > MaxX) MaxX = pt.x(); + if (pt.y() < MinY) MinY = pt.y(); + if (pt.y() > MaxY) MaxY = pt.y(); + if (pt.z() < MinZ) MinZ = pt.z(); + if (pt.z() > MaxZ) MaxZ = pt.z(); } - mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); - mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); - + mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ); + mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ); - -// const btScalar r = m_radius / sqrtf(2.0f); + // const btScalar r = m_radius / sqrtf(2.0f); const btScalar r = m_radius / sqrtf(3.0f); const int LargestExtent = mE.maxAxis(); - const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f; + const btScalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f; m_extents[0] = m_extents[1] = m_extents[2] = r; - m_extents[LargestExtent] = mE[LargestExtent]*0.5f; + m_extents[LargestExtent] = mE[LargestExtent] * 0.5f; bool FoundBox = false; - for(int j=0;j<1024;j++) + for (int j = 0; j < 1024; j++) { - if(testContainment()) + if (testContainment()) { FoundBox = true; break; @@ -245,25 +245,25 @@ void btConvexPolyhedron::initialize() m_extents[LargestExtent] -= Step; } - if(!FoundBox) + if (!FoundBox) { m_extents[0] = m_extents[1] = m_extents[2] = r; } else { // Refine the box - const btScalar Step = (m_radius - r)/1024.0f; - const int e0 = (1<<LargestExtent) & 3; - const int e1 = (1<<e0) & 3; + const btScalar Step = (m_radius - r) / 1024.0f; + const int e0 = (1 << LargestExtent) & 3; + const int e1 = (1 << e0) & 3; - for(int j=0;j<1024;j++) + for (int j = 0; j < 1024; j++) { const btScalar Saved0 = m_extents[e0]; const btScalar Saved1 = m_extents[e1]; m_extents[e0] += Step; m_extents[e1] += Step; - if(!testContainment()) + if (!testContainment()) { m_extents[e0] = Saved0; m_extents[e1] = Saved1; @@ -274,30 +274,29 @@ void btConvexPolyhedron::initialize() } #endif } - -void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const { minProj = FLT_MAX; maxProj = -FLT_MAX; int numVerts = m_vertices.size(); - for(int i=0;i<numVerts;i++) + for (int i = 0; i < numVerts; i++) { btVector3 pt = trans * m_vertices[i]; btScalar dp = pt.dot(dir); - if(dp < minProj) + if (dp < minProj) { minProj = dp; witnesPtMin = pt; } - if(dp > maxProj) + if (dp > maxProj) { maxProj = dp; witnesPtMax = pt; } } - if(minProj>maxProj) + if (minProj > maxProj) { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); + btSwap(minProj, maxProj); + btSwap(witnesPtMin, witnesPtMax); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h index d3cd066ac8d..638aa9b3df0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -13,10 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans - #ifndef _BT_POLYHEDRAL_FEATURES_H #define _BT_POLYHEDRAL_FEATURES_H @@ -25,41 +23,37 @@ subject to the following restrictions: #define TEST_INTERNAL_OBJECTS 1 - struct btFace { - btAlignedObjectArray<int> m_indices; -// btAlignedObjectArray<int> m_connectedFaces; - btScalar m_plane[4]; + btAlignedObjectArray<int> m_indices; + // btAlignedObjectArray<int> m_connectedFaces; + btScalar m_plane[4]; }; - -ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron +ATTRIBUTE_ALIGNED16(class) +btConvexPolyhedron { - public: - +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btConvexPolyhedron(); - virtual ~btConvexPolyhedron(); + virtual ~btConvexPolyhedron(); - btAlignedObjectArray<btVector3> m_vertices; - btAlignedObjectArray<btFace> m_faces; + btAlignedObjectArray<btVector3> m_vertices; + btAlignedObjectArray<btFace> m_faces; btAlignedObjectArray<btVector3> m_uniqueEdges; - btVector3 m_localCenter; - btVector3 m_extents; - btScalar m_radius; - btVector3 mC; - btVector3 mE; + btVector3 m_localCenter; + btVector3 m_extents; + btScalar m_radius; + btVector3 mC; + btVector3 mE; - void initialize(); + void initialize(); + void initialize2(); bool testContainment() const; - void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; }; - -#endif //_BT_POLYHEDRAL_FEATURES_H - - +#endif //_BT_POLYHEDRAL_FEATURES_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 88018b4c624..f8fb0aa9fda 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -13,9 +13,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "btConvexShape.h" #include "btTriangleShape.h" @@ -27,30 +27,28 @@ subject to the following restrictions: #include "btConvexPointCloudShape.h" ///not supported on IBM SDK, until we fix the alignment of btVector3 -#if defined (__CELLOS_LV2__) && defined (__SPU__) +#if defined(__CELLOS_LV2__) && defined(__SPU__) #include <spu_intrinsics.h> -static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) +static inline vec_float4 vec_dot3(vec_float4 vec0, vec_float4 vec1) { - vec_float4 result; - result = spu_mul( vec0, vec1 ); - result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); - return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); + vec_float4 result; + result = spu_mul(vec0, vec1); + result = spu_madd(spu_rlqwbyte(vec0, 4), spu_rlqwbyte(vec1, 4), result); + return spu_madd(spu_rlqwbyte(vec0, 8), spu_rlqwbyte(vec1, 8), result); } -#endif //__SPU__ +#endif //__SPU__ -btConvexShape::btConvexShape () +btConvexShape::btConvexShape() { } btConvexShape::~btConvexShape() { - } - -void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin, btVector3& witnesPtMax) const { - btVector3 localAxis = dir*trans.getBasis(); + btVector3 localAxis = dir * trans.getBasis(); btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); @@ -58,8 +56,8 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc max = vtx2.dot(dir); witnesPtMax = vtx2; witnesPtMin = vtx1; - - if(min>max) + + if (min > max) { btScalar tmp = min; min = max; @@ -69,391 +67,392 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc } } - -static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) -{ - +static btVector3 convexHullSupport(const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) +{ btVector3 vec = localDirOrg * localScaling; -#if defined (__CELLOS_LV2__) && defined (__SPU__) +#if defined(__CELLOS_LV2__) && defined(__SPU__) btVector3 localDir = vec; - vec_float4 v_distMax = {-FLT_MAX,0,0,0}; - vec_int4 v_idxMax = {-999,0,0,0}; - int v=0; + vec_float4 v_distMax = {-FLT_MAX, 0, 0, 0}; + vec_int4 v_idxMax = {-999, 0, 0, 0}; + int v = 0; int numverts = numPoints; - for(;v<(int)numverts-4;v+=4) { - vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); - vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); - vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); - vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); - const vec_int4 i0 = {v ,0,0,0}; - const vec_int4 i1 = {v+1,0,0,0}; - const vec_int4 i2 = {v+2,0,0,0}; - const vec_int4 i3 = {v+3,0,0,0}; - vec_uint4 retGt01 = spu_cmpgt(p0,p1); - vec_float4 pmax01 = spu_sel(p1,p0,retGt01); - vec_int4 imax01 = spu_sel(i1,i0,retGt01); - vec_uint4 retGt23 = spu_cmpgt(p2,p3); - vec_float4 pmax23 = spu_sel(p3,p2,retGt23); - vec_int4 imax23 = spu_sel(i3,i2,retGt23); - vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); - vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); - vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); - vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); - v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); - v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); + for (; v < (int)numverts - 4; v += 4) + { + vec_float4 p0 = vec_dot3(points[v].get128(), localDir.get128()); + vec_float4 p1 = vec_dot3(points[v + 1].get128(), localDir.get128()); + vec_float4 p2 = vec_dot3(points[v + 2].get128(), localDir.get128()); + vec_float4 p3 = vec_dot3(points[v + 3].get128(), localDir.get128()); + const vec_int4 i0 = {v, 0, 0, 0}; + const vec_int4 i1 = {v + 1, 0, 0, 0}; + const vec_int4 i2 = {v + 2, 0, 0, 0}; + const vec_int4 i3 = {v + 3, 0, 0, 0}; + vec_uint4 retGt01 = spu_cmpgt(p0, p1); + vec_float4 pmax01 = spu_sel(p1, p0, retGt01); + vec_int4 imax01 = spu_sel(i1, i0, retGt01); + vec_uint4 retGt23 = spu_cmpgt(p2, p3); + vec_float4 pmax23 = spu_sel(p3, p2, retGt23); + vec_int4 imax23 = spu_sel(i3, i2, retGt23); + vec_uint4 retGt0123 = spu_cmpgt(pmax01, pmax23); + vec_float4 pmax0123 = spu_sel(pmax23, pmax01, retGt0123); + vec_int4 imax0123 = spu_sel(imax23, imax01, retGt0123); + vec_uint4 retGtMax = spu_cmpgt(v_distMax, pmax0123); + v_distMax = spu_sel(pmax0123, v_distMax, retGtMax); + v_idxMax = spu_sel(imax0123, v_idxMax, retGtMax); } - for(;v<(int)numverts;v++) { - vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); - const vec_int4 i = {v,0,0,0}; - vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); - v_distMax = spu_sel(p,v_distMax,retGtMax); - v_idxMax = spu_sel(i,v_idxMax,retGtMax); + for (; v < (int)numverts; v++) + { + vec_float4 p = vec_dot3(points[v].get128(), localDir.get128()); + const vec_int4 i = {v, 0, 0, 0}; + vec_uint4 retGtMax = spu_cmpgt(v_distMax, p); + v_distMax = spu_sel(p, v_distMax, retGtMax); + v_idxMax = spu_sel(i, v_idxMax, retGtMax); } - int ptIndex = spu_extract(v_idxMax,0); - const btVector3& supVec= points[ptIndex] * localScaling; + int ptIndex = spu_extract(v_idxMax, 0); + const btVector3& supVec = points[ptIndex] * localScaling; return supVec; #else - btScalar maxDot; - long ptIndex = vec.maxDot( points, numPoints, maxDot); + btScalar maxDot; + long ptIndex = vec.maxDot(points, numPoints, maxDot); btAssert(ptIndex >= 0); + if (ptIndex < 0) + { + ptIndex = 0; + } btVector3 supVec = points[ptIndex] * localScaling; return supVec; -#endif //__SPU__ +#endif //__SPU__ } -btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const +btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual(const btVector3& localDir) const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - return btVector3(0,0,0); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); - -#if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON )) - #if defined( BT_USE_SSE ) - return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 )); - #elif defined( BT_USE_NEON ) - return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 )); - #else - #error unknown vector arch - #endif -#else - return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), - btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), - btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); -#endif - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3* vertices = &triangleShape->m_vertices1[0]; - btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]); - btVector3 sup = vertices[dots.maxAxis()]; - return btVector3(sup.getX(),sup.getY(),sup.getZ()); - } - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)this; - //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - - btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); - btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); - int cylinderUpAxis = cylShape->getUpAxis(); - int XX(1),YY(0),ZZ(2); - - switch (cylinderUpAxis) + case SPHERE_SHAPE_PROXYTYPE: { - case 0: - { - XX = 1; - YY = 0; - ZZ = 2; + return btVector3(0, 0, 0); } - break; - case 1: + case BOX_SHAPE_PROXYTYPE: { - XX = 0; - YY = 1; - ZZ = 2; + btBoxShape* convexShape = (btBoxShape*)this; + const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); + +#if defined(__APPLE__) && (defined(BT_USE_SSE) || defined(BT_USE_NEON)) +#if defined(BT_USE_SSE) + return btVector3(_mm_xor_ps(_mm_and_ps(localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f}), halfExtents.mVec128)); +#elif defined(BT_USE_NEON) + return btVector3((float32x4_t)(((uint32x4_t)localDir.mVec128 & (uint32x4_t){0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t)halfExtents.mVec128)); +#else +#error unknown vector arch +#endif +#else + return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), + btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), + btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); +#endif } - break; - case 2: + case TRIANGLE_SHAPE_PROXYTYPE: { - XX = 0; - YY = 2; - ZZ = 1; - + btTriangleShape* triangleShape = (btTriangleShape*)this; + btVector3 dir(localDir.getX(), localDir.getY(), localDir.getZ()); + btVector3* vertices = &triangleShape->m_vertices1[0]; + btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]); + btVector3 sup = vertices[dots.maxAxis()]; + return btVector3(sup.getX(), sup.getY(), sup.getZ()); } - break; - default: - btAssert(0); - break; - }; - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - btVector3 tmp; - btScalar d ; + btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); + btVector3 v(localDir.getX(), localDir.getY(), localDir.getZ()); + int cylinderUpAxis = cylShape->getUpAxis(); + int XX(1), YY(0), ZZ(2); - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } else { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); + switch (cylinderUpAxis) + { + case 0: + { + XX = 1; + YY = 0; + ZZ = 2; + } + break; + case 1: + { + XX = 0; + YY = 1; + ZZ = 2; + } + break; + case 2: + { + XX = 0; + YY = 2; + ZZ = 1; + } + break; + default: + btAssert(0); + break; + }; + + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; + + btVector3 tmp; + btScalar d; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return btVector3(tmp.getX(), tmp.getY(), tmp.getZ()); + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return btVector3(tmp.getX(), tmp.getY(), tmp.getZ()); + } } - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + case CAPSULE_SHAPE_PROXYTYPE: + { + btVector3 vec0(localDir.getX(), localDir.getY(), localDir.getZ()); - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btScalar halfHeight = capsuleShape->getHalfHeight(); - int capsuleUpAxis = capsuleShape->getUpAxis(); + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btScalar halfHeight = capsuleShape->getHalfHeight(); + int capsuleUpAxis = capsuleShape->getUpAxis(); - btScalar radius = capsuleShape->getRadius(); - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); - btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - btVector3 vtx; - btScalar newDot; - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = halfHeight; + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < SIMD_EPSILON * SIMD_EPSILON) + { + vec.setValue(1, 0, 0); + } + else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); + vec *= rlen; + } + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0, 0, 0); + pos[capsuleUpAxis] = halfHeight; - //vtx = pos +vec*(radius); - vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); - newDot = vec.dot(vtx); - + vtx = pos; + newDot = vec.dot(vtx); - if (newDot > maxDot) + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } { - maxDot = newDot; - supVec = vtx; + btVector3 pos(0, 0, 0); + pos[capsuleUpAxis] = -halfHeight; + + vtx = pos; + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } } + return btVector3(supVec.getX(), supVec.getY(), supVec.getZ()); } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = -halfHeight; - - //vtx = pos +vec*(radius); - vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } + btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; + btVector3* points = convexPointCloudShape->getUnscaledPoints(); + int numPoints = convexPointCloudShape->getNumPoints(); + return convexHullSupport(localDir, points, numPoints, convexPointCloudShape->getLocalScalingNV()); } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - } - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - { - btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; - btVector3* points = convexPointCloudShape->getUnscaledPoints (); - int numPoints = convexPointCloudShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV()); - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btConvexHullShape* convexHullShape = (btConvexHullShape*)this; - btVector3* points = convexHullShape->getUnscaledPoints(); - int numPoints = convexHullShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV()); - } - default: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexHullShape* convexHullShape = (btConvexHullShape*)this; + btVector3* points = convexHullShape->getUnscaledPoints(); + int numPoints = convexHullShape->getNumPoints(); + return convexHullSupport(localDir, points, numPoints, convexHullShape->getLocalScalingNV()); + } + default: #ifndef __SPU__ - return this->localGetSupportingVertexWithoutMargin (localDir); + return this->localGetSupportingVertexWithoutMargin(localDir); #else - btAssert (0); + btAssert(0); #endif } // should never reach here - btAssert (0); - return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); + btAssert(0); + return btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); } -btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const +btVector3 btConvexShape::localGetSupportVertexNonVirtual(const btVector3& localDir) const { btVector3 localDirNorm = localDir; - if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (localDirNorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + localDirNorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); } - localDirNorm.normalize (); + localDirNorm.normalize(); - return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; + return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm) + getMarginNonVirtual() * localDirNorm; } /* TODO: This should be bumped up to btCollisionShape () */ -btScalar btConvexShape::getMarginNonVirtual () const +btScalar btConvexShape::getMarginNonVirtual() const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - return sphereShape->getRadius (); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - return convexShape->getMarginNV (); - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - return triangleShape->getMarginNV (); - } - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)this; - return cylShape->getMarginNV(); - } - case CONE_SHAPE_PROXYTYPE: - { - btConeShape* conShape = (btConeShape*)this; - return conShape->getMarginNV(); - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - return capsuleShape->getMarginNV(); - } - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - /* fall through */ - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; - return convexHullShape->getMarginNV(); - } - default: + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphereShape = (btSphereShape*)this; + return sphereShape->getRadius(); + } + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + return convexShape->getMarginNV(); + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + return triangleShape->getMarginNV(); + } + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + return cylShape->getMarginNV(); + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShape* conShape = (btConeShape*)this; + return conShape->getMarginNV(); + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + return capsuleShape->getMarginNV(); + } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + /* fall through */ + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; + return convexHullShape->getMarginNV(); + } + default: #ifndef __SPU__ - return this->getMargin (); + return this->getMargin(); #else - btAssert (0); + btAssert(0); #endif } // should never reach here - btAssert (0); + btAssert(0); return btScalar(0.0f); } #ifndef __SPU__ -void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const +void btConvexShape::getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); - btScalar margin = radius + sphereShape->getMarginNonVirtual(); - const btVector3& center = t.getOrigin(); - btVector3 extent(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - } - break; - case CYLINDER_SHAPE_PROXYTYPE: - /* fall through */ - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - btScalar margin=convexShape->getMarginNonVirtual(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - halfExtents += btVector3(margin,margin,margin); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - - aabbMin = center - extent; - aabbMax = center + extent; + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphereShape = (btSphereShape*)this; + btScalar radius = sphereShape->getImplicitShapeDimensions().getX(); // * convexShape->getLocalScaling().getX(); + btScalar margin = radius + sphereShape->getMarginNonVirtual(); + const btVector3& center = t.getOrigin(); + btVector3 extent(margin, margin, margin); + aabbMin = center - extent; + aabbMax = center + extent; + } break; - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - btScalar margin = triangleShape->getMarginNonVirtual(); - for (int i=0;i<3;i++) + case CYLINDER_SHAPE_PROXYTYPE: + /* fall through */ + case BOX_SHAPE_PROXYTYPE: { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); + btBoxShape* convexShape = (btBoxShape*)this; + btScalar margin = convexShape->getMarginNonVirtual(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + halfExtents += btVector3(margin, margin, margin); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + btScalar margin = triangleShape->getMarginNonVirtual(); + for (int i = 0; i < 3; i++) + { + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); + vec[i] = btScalar(1.); - btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); + btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis()); - btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; - vec[i] = btScalar(-1.); - tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); - aabbMin[i] = tmp[i]-margin; - } - } - break; - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); - int m_upAxis = capsuleShape->getUpAxis(); - halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); - halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center - extent; - aabbMax = center + extent; - } - break; - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; - btScalar margin = convexHullShape->getMarginNonVirtual(); - convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); - } - break; - default: + btVector3 tmp = t(sv); + aabbMax[i] = tmp[i] + margin; + vec[i] = btScalar(-1.); + tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis())); + aabbMin[i] = tmp[i] - margin; + } + } + break; + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btVector3 halfExtents(capsuleShape->getRadius(), capsuleShape->getRadius(), capsuleShape->getRadius()); + int m_upAxis = capsuleShape->getUpAxis(); + halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMin = center - extent; + aabbMax = center + extent; + } + break; + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; + btScalar margin = convexHullShape->getMarginNonVirtual(); + convexHullShape->getNonvirtualAabb(t, aabbMin, aabbMax, margin); + } + break; + default: #ifndef __SPU__ - this->getAabb (t, aabbMin, aabbMax); + this->getAabb(t, aabbMin, aabbMax); #else - btAssert (0); + btAssert(0); #endif - break; + break; } // should never reach here - btAssert (0); + btAssert(0); } -#endif //__SPU__ +#endif //__SPU__ diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 875f2ac195a..d3b3ed816ec 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -28,58 +28,48 @@ subject to the following restrictions: /// 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 +ATTRIBUTE_ALIGNED16(class) +btConvexShape : public btCollisionShape { - - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btConvexShape (); + btConvexShape(); virtual ~btConvexShape(); - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const = 0; - - //////// - #ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0; - #endif //#ifndef __SPU__ + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const = 0; - btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const; - btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const; - btScalar getMarginNonVirtual () const; - void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; +//////// +#ifndef __SPU__ + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const = 0; +#endif //#ifndef __SPU__ + btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3& vec) const; + btVector3 localGetSupportVertexNonVirtual(const btVector3& vec) const; + btScalar getMarginNonVirtual() const; + void getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const = 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 =0; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void setLocalScaling(const btVector3& scaling) =0; - virtual const btVector3& getLocalScaling() const =0; + virtual void setLocalScaling(const btVector3& scaling) = 0; + virtual const btVector3& getLocalScaling() const = 0; - virtual void setMargin(btScalar margin)=0; + virtual void setMargin(btScalar margin) = 0; - virtual btScalar getMargin() const=0; + virtual btScalar getMargin() const = 0; - virtual int getNumPreferredPenetrationDirections() const=0; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0; + virtual int getNumPreferredPenetrationDirections() const = 0; - - - + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const = 0; }; - - -#endif //BT_CONVEX_SHAPE_INTERFACE1 +#endif //BT_CONVEX_SHAPE_INTERFACE1 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 0f9ced554b8..f6987cc760c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -19,42 +19,37 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" - -btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) +btConvexTriangleMeshShape ::btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb) + : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) { m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; - if ( calcAabb ) + if (calcAabb) recalcLocalAabb(); } - - - ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once ///but then we are duplicating -class LocalSupportVertexCallback: public btInternalTriangleIndexCallback +class LocalSupportVertexCallback : public btInternalTriangleIndexCallback { - btVector3 m_supportVertexLocal; -public: +public: btScalar m_maxDot; btVector3 m_supportVecLocal; LocalSupportVertexCallback(const btVector3& supportVecLocal) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), - m_maxDot(btScalar(-BT_LARGE_FLOAT)), - m_supportVecLocal(supportVecLocal) + : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)), + m_maxDot(btScalar(-BT_LARGE_FLOAT)), + m_supportVecLocal(supportVecLocal) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { (void)triangleIndex; (void)partId; - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); if (dot > m_maxDot) @@ -64,99 +59,82 @@ public: } } } - - btVector3 GetSupportVertexLocal() + + btVector3 GetSupportVertexLocal() { return m_supportVertexLocal; } - }; - - - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } - LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax); supVec = supportCallback.GetSupportVertexLocal(); return supVec; } -void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { //use 'w' component of supportVerticesOut? { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } } - - ///@todo: could do the batch inside the callback! + ///@todo: could do the batch inside the callback! - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { const btVector3& vec = vectors[j]; - LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); } - } - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - - - - - - - - //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo -int btConvexTriangleMeshShape::getNumVertices() const +int btConvexTriangleMeshShape::getNumVertices() const { //cache this? return 0; - } int btConvexTriangleMeshShape::getNumEdges() const @@ -164,44 +142,40 @@ int btConvexTriangleMeshShape::getNumEdges() const return 0; } -void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const +void btConvexTriangleMeshShape::getEdge(int, btVector3&, btVector3&) const { - btAssert(0); + btAssert(0); } -void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const +void btConvexTriangleMeshShape::getVertex(int, btVector3&) const { btAssert(0); } -int btConvexTriangleMeshShape::getNumPlanes() const +int btConvexTriangleMeshShape::getNumPlanes() const { return 0; } -void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexTriangleMeshShape::getPlane(btVector3&, btVector3&, int) const { btAssert(0); } //not yet -bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const +bool btConvexTriangleMeshShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } - - -void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_stridingMesh->setScaling(scaling); - + recalcLocalAabb(); - } - const btVector3& btConvexTriangleMeshShape::getLocalScaling() const { return m_stridingMesh->getScaling(); @@ -209,107 +183,101 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const 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; - 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 * (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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - 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; -} + 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; + 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 * (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(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + 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 f338865ca13..6dac9fff040 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -15,24 +15,22 @@ subject to the following restrictions: #ifndef BT_CONVEX_TRIANGLEMESH_SHAPE_H #define BT_CONVEX_TRIANGLEMESH_SHAPE_H - #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types /// 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. -ATTRIBUTE_ALIGNED16(class) btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape { - - class btStridingMeshInterface* m_stridingMesh; + class btStridingMeshInterface* m_stridingMesh; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true); - class btStridingMeshInterface* getMeshInterface() + btConvexTriangleMeshShape(btStridingMeshInterface * meshInterface, bool calcAabb = true); + + class btStridingMeshInterface* getMeshInterface() { return m_stridingMesh; } @@ -40,24 +38,23 @@ public: { return m_stridingMesh; } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + //debugging - virtual const char* getName()const {return "ConvexTrimesh";} - - virtual int getNumVertices() const; + virtual const char* getName() const { return "ConvexTrimesh"; } + + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; - - virtual void setLocalScaling(const btVector3& scaling); + 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 @@ -65,13 +62,7 @@ public: ///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; - + void calculatePrincipalAxisTransform(btTransform & principal, btVector3 & inertia, btScalar & volume) const; }; - - -#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H - - - +#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 6cfe43be4da..66dbb8e53de 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -15,41 +15,37 @@ subject to the following restrictions: #include "btCylinderShape.h" -btCylinderShape::btCylinderShape (const btVector3& halfExtents) -:btConvexInternalShape(), -m_upAxis(1) +btCylinderShape::btCylinderShape(const btVector3& halfExtents) + : btConvexInternalShape(), + m_upAxis(1) { + btVector3 margin(getMargin(), getMargin(), getMargin()); + m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; + setSafeMargin(halfExtents); - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; m_shapeType = CYLINDER_SHAPE_PROXYTYPE; } - -btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) -:btCylinderShape(halfExtents) +btCylinderShapeX::btCylinderShapeX(const btVector3& halfExtents) + : btCylinderShape(halfExtents) { m_upAxis = 0; - } - -btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) -:btCylinderShape(halfExtents) +btCylinderShapeZ::btCylinderShapeZ(const btVector3& halfExtents) + : btCylinderShape(halfExtents) { m_upAxis = 2; - } -void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btCylinderShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } -void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCylinderShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility //#define USE_BOX_INERTIA_APPROXIMATION 1 #ifndef USE_BOX_INERTIA_APPROXIMATION @@ -63,25 +59,25 @@ void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co * */ - btScalar radius2; // square of cylinder radius - btScalar height2; // square of cylinder height - btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension + btScalar radius2; // square of cylinder radius + btScalar height2; // square of cylinder height + btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension btScalar div12 = mass / 12.f; btScalar div4 = mass / 4.f; btScalar div2 = mass / 2.f; int idxRadius, idxHeight; - switch (m_upAxis) // get indices of radius and height of cylinder + switch (m_upAxis) // get indices of radius and height of cylinder { - case 0: // cylinder is aligned along x + case 0: // cylinder is aligned along x idxRadius = 1; idxHeight = 0; break; - case 2: // cylinder is aligned along z + case 2: // cylinder is aligned along z idxRadius = 0; idxHeight = 2; break; - default: // cylinder is aligned along y + default: // cylinder is aligned along y idxRadius = 0; idxHeight = 1; } @@ -94,188 +90,164 @@ void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co btScalar t1 = div12 * height2 + div4 * radius2; btScalar t2 = div2 * radius2; - switch (m_upAxis) // set diagonal elements of inertia tensor + switch (m_upAxis) // set diagonal elements of inertia tensor { - case 0: // cylinder is aligned along x - inertia.setValue(t2,t1,t1); + case 0: // cylinder is aligned along x + inertia.setValue(t2, t1, t1); break; - case 2: // cylinder is aligned along z - inertia.setValue(t1,t1,t2); + case 2: // cylinder is aligned along z + inertia.setValue(t1, t1, t2); break; - default: // cylinder is aligned along y - inertia.setValue(t1,t2,t1); + default: // cylinder is aligned along y + inertia.setValue(t1, t2, t1); } -#else //USE_BOX_INERTIA_APPROXIMATION +#else //USE_BOX_INERTIA_APPROXIMATION //approximation of box shape btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); -#endif //USE_BOX_INERTIA_APPROXIMATION + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); +#endif //USE_BOX_INERTIA_APPROXIMATION } - -SIMD_FORCE_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; -const int YY = 0; -const int ZZ = 2; + const int cylinderUpAxis = 0; + const int XX = 1; + const int YY = 0; + const int ZZ = 2; //mapping depends on how cylinder local orientation is // extents of the cylinder is: X,Y is for radius, and Z for height - btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - - + } } - - - - - -inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) +inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents, const btVector3& v) { - -const int cylinderUpAxis = 1; -const int XX = 0; -const int YY = 1; -const int ZZ = 2; - + const int cylinderUpAxis = 1; + const int XX = 0; + const int YY = 1; + const int ZZ = 2; btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - + } } -inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) +inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents, const btVector3& v) { -const int cylinderUpAxis = 2; -const int XX = 0; -const int YY = 2; -const int ZZ = 1; + const int cylinderUpAxis = 2; + const int XX = 0; + const int YY = 2; + const int ZZ = 1; //mapping depends on how cylinder local orientation is // extents of the cylinder is: X,Y is for radius, and Z for height - btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - - + } } -btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec); + return CylinderLocalSupportX(getHalfExtentsWithoutMargin(), vec); } - -btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec); + return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vec); } -btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec); + return CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vec); } -void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vectors[i]); } } -void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vectors[i]); } } - - - -void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),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 6f796950e1d..d3f64508bae 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -17,90 +17,84 @@ subject to the following restrictions: #define BT_CYLINDER_MINKOWSKI_H #include "btBoxShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btVector3.h" /// 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. -ATTRIBUTE_ALIGNED16(class) btCylinderShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btCylinderShape : public btConvexInternalShape { - protected: - - int m_upAxis; + int m_upAxis; public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + 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 + return m_implicitShapeDimensions; //changed in Bullet 2.63: assume the scaling and margin are included } - btCylinderShape (const btVector3& halfExtents); - - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + btCylinderShape(const btVector3& halfExtents); + + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; virtual void setMargin(btScalar collisionMargin) { //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; + btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); + btVector3 newMargin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; - } - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { - btVector3 supVertex; supVertex = localGetSupportingVertexWithoutMargin(vec); - - if ( getMargin()!=btScalar(0.) ) + + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - //use box inertia // virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - int getUpAxis() const + int getUpAxis() const { return m_upAxis; } - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - btVector3 aniDir(0,0,0); - aniDir[getUpAxis()]=1; + btVector3 aniDir(0, 0, 0); + aniDir[getUpAxis()] = 1; return aniDir; } @@ -109,43 +103,41 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); return getHalfExtentsWithMargin().getX(); } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; btConvexInternalShape::setLocalScaling(scaling); m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; - } //debugging - virtual const char* getName()const + virtual const char* getName() const { return "CylinderY"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; class btCylinderShapeX : public btCylinderShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCylinderShapeX (const btVector3& halfExtents); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - //debugging - virtual const char* getName()const + + btCylinderShapeX(const btVector3& halfExtents); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + //debugging + virtual const char* getName() const { return "CylinderX"; } @@ -154,21 +146,20 @@ public: { return getHalfExtentsWithMargin().getY(); } - }; class btCylinderShapeZ : public btCylinderShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCylinderShapeZ (const btVector3& halfExtents); - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + btCylinderShapeZ(const btVector3& halfExtents); - //debugging - virtual const char* getName()const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + //debugging + virtual const char* getName() const { return "CylinderZ"; } @@ -177,37 +168,39 @@ public: { return getHalfExtentsWithMargin().getX(); } - }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCylinderShapeData +struct btCylinderShapeData { - btConvexInternalShapeData m_convexInternalShapeData; + btConvexInternalShapeData m_convexInternalShapeData; - int m_upAxis; + int m_upAxis; - char m_padding[4]; + char m_padding[4]; }; -SIMD_FORCE_INLINE int btCylinderShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCylinderShape::calculateSerializeBufferSize() const { return sizeof(btCylinderShapeData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer; - - btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); + btCylinderShapeData* shapeData = (btCylinderShapeData*)dataBuffer; - shapeData->m_upAxis = m_upAxis; - - return "btCylinderShapeData"; -} + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); + shapeData->m_upAxis = m_upAxis; + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; -#endif //BT_CYLINDER_MINKOWSKI_H + return "btCylinderShapeData"; +} +#endif //BT_CYLINDER_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index a9e6df5c58e..4699555bd87 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -15,36 +15,28 @@ subject to the following restrictions: #include "btEmptyShape.h" - #include "btCollisionShape.h" - -btEmptyShape::btEmptyShape() : btConcaveShape () +btEmptyShape::btEmptyShape() : btConcaveShape() { m_shapeType = EMPTY_SHAPE_PROXYTYPE; } - btEmptyShape::~btEmptyShape() { } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btEmptyShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); aabbMin = t.getOrigin() - margin; aabbMax = t.getOrigin() + margin; - } -void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const +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 069a79402bf..d2e21173b21 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -23,50 +23,43 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "btCollisionMargin.h" - - - /// 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. -ATTRIBUTE_ALIGNED16(class) btEmptyShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btEmptyShape : public btConcaveShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btEmptyShape(); virtual ~btEmptyShape(); - ///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; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } - virtual const btVector3& getLocalScaling() const + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual const char* getName()const + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + + virtual const char* getName() const { return "Empty"; } - virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const + virtual void processAllTriangles(btTriangleCallback*, const btVector3&, const btVector3&) const { } protected: - btVector3 m_localScaling; - + btVector3 m_localScaling; }; - - -#endif //BT_EMPTY_SHAPE_H +#endif //BT_EMPTY_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 441a89c6bb6..34e7926f17c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -17,23 +17,23 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) + :m_userIndex2(-1), + m_userValue3(0), + m_triangleInfoMap(0) { initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges) + :m_userIndex2(-1), + m_userValue3(0), + m_triangleInfoMap(0) { // legacy constructor: support only float or unsigned char, // and min height is zero @@ -45,27 +45,23 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h btScalar heightScale = maxHeight / 65535; initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -void btHeightfieldTerrainShape::initialize -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +void btHeightfieldTerrainShape::initialize( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) { // validation - btAssert(heightStickWidth > 1);// && "bad width"); - btAssert(heightStickLength > 1);// && "bad length"); - btAssert(heightfieldData);// && "null heightfield data"); + btAssert(heightStickWidth > 1); // && "bad width"); + btAssert(heightStickLength > 1); // && "bad length"); + btAssert(heightfieldData); // && "null heightfield data"); // btAssert(heightScale) -- do we care? Trust caller here - btAssert(minHeight <= maxHeight);// && "bad min/max height"); - btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum"); + btAssert(minHeight <= maxHeight); // && "bad min/max height"); + btAssert(upAxis >= 0 && upAxis < 3); // && "bad upAxis--should be in range [0,2]"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT); // && "Bad height data type enum"); // initialize member variables m_shapeType = TERRAIN_SHAPE_PROXYTYPE; @@ -73,42 +69,47 @@ PHY_ScalarType hdt, bool flipQuadEdges m_heightStickLength = heightStickLength; m_minHeight = minHeight; m_maxHeight = maxHeight; - m_width = (btScalar) (heightStickWidth - 1); - m_length = (btScalar) (heightStickLength - 1); + m_width = (btScalar)(heightStickWidth - 1); + m_length = (btScalar)(heightStickLength - 1); m_heightScale = heightScale; m_heightfieldDataUnknown = heightfieldData; m_heightDataType = hdt; m_flipQuadEdges = flipQuadEdges; m_useDiamondSubdivision = false; m_useZigzagSubdivision = false; + m_flipTriangleWinding = false; m_upAxis = upAxis; m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); + + m_vboundsChunkSize = 0; + m_vboundsGridWidth = 0; + m_vboundsGridLength = 0; // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) { - case 0: + case 0: { m_localAabbMin.setValue(m_minHeight, 0, 0); m_localAabbMax.setValue(m_maxHeight, m_width, m_length); break; } - case 1: + case 1: { m_localAabbMin.setValue(0, m_minHeight, 0); m_localAabbMax.setValue(m_width, m_maxHeight, m_length); break; }; - case 2: + case 2: { m_localAabbMin.setValue(0, 0, m_minHeight); m_localAabbMax.setValue(m_width, m_length, m_maxHeight); break; } - default: + default: { //need to get valid m_upAxis - btAssert(0);// && "Bad m_upAxis"); + btAssert(0); // && "Bad m_upAxis"); } } @@ -116,62 +117,58 @@ PHY_ScalarType hdt, bool flipQuadEdges m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); } - - btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { + clearAccelerator(); } - - -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + btVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * btScalar(0.5); btVector3 localOrigin(0, 0, 0); localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); localOrigin *= m_localScaling; - btMatrix3x3 abs_b = t.getBasis().absolute(); + btMatrix3x3 abs_b = t.getBasis().absolute(); btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - extent += btVector3(getMargin(),getMargin(),getMargin()); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + extent += btVector3(getMargin(), getMargin(), getMargin()); aabbMin = center - extent; aabbMax = center + extent; } - /// This returns the "raw" (user's initial) height, not the actual height. /// The actual height needs to be adjusted to be relative to the center /// of the heightfield's AABB. btScalar -btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const +btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const { btScalar val = 0.f; switch (m_heightDataType) { - case PHY_FLOAT: + case PHY_FLOAT: { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; + val = m_heightfieldDataFloat[(y * m_heightStickWidth) + x]; break; } - case PHY_UCHAR: + case PHY_UCHAR: { - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x]; val = heightFieldValue * m_heightScale; break; } - case PHY_SHORT: + case PHY_SHORT: { short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; val = hfValue * m_heightScale; break; } - default: + default: { btAssert(!"Bad m_heightDataType"); } @@ -180,74 +177,63 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const return val; } - - - /// this returns the vertex in bullet-local coordinates -void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const +void btHeightfieldTerrainShape::getVertex(int x, int y, btVector3& vertex) const { - btAssert(x>=0); - btAssert(y>=0); - btAssert(x<m_heightStickWidth); - btAssert(y<m_heightStickLength); + btAssert(x >= 0); + btAssert(y >= 0); + btAssert(x < m_heightStickWidth); + btAssert(y < m_heightStickLength); - btScalar height = getRawHeightFieldValue(x,y); + btScalar height = getRawHeightFieldValue(x, y); switch (m_upAxis) { - case 0: + case 0: { - vertex.setValue( - height - m_localOrigin.getX(), - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0) ) + y - ); + vertex.setValue( + height - m_localOrigin.getX(), + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y); break; } - case 1: + case 1: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - height - m_localOrigin.getY(), - (-m_length/btScalar(2.0)) + y - ); + (-m_width / btScalar(2.0)) + x, + height - m_localOrigin.getY(), + (-m_length / btScalar(2.0)) + y); break; }; - case 2: + case 2: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0)) + y, - height - m_localOrigin.getZ() - ); + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y, + height - m_localOrigin.getZ()); break; } - default: + default: { //need to get valid m_upAxis btAssert(0); } } - vertex*=m_localScaling; + vertex *= m_localScaling; } - - static inline int -getQuantized -( -btScalar x -) +getQuantized( + btScalar x) { - if (x < 0.0) { - return (int) (x - 0.5); + if (x < 0.0) + { + return (int)(x - 0.5); } - return (int) (x + 0.5); + return (int)(x + 0.5); } - - /// given input vector, return quantized version /** This routine is basically determining the gridpoint indices for a given @@ -257,7 +243,7 @@ btScalar x "with clamp" means that we restrict the point to be in the heightfield's axis-aligned bounding box. */ -void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const +void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point, int /*isMax*/) const { btVector3 clampedPoint(point); clampedPoint.setMax(m_localAabbMin); @@ -266,11 +252,8 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi out[0] = getQuantized(clampedPoint.getX()); out[1] = getQuantized(clampedPoint.getY()); out[2] = getQuantized(clampedPoint.getZ()); - } - - /// process all triangles within the provided axis-aligned bounding box /** basic algorithm: @@ -278,128 +261,132 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi - convert input aabb to a range of heightfield grid points (quantize) - iterate over all triangles in that subset of the grid */ -void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { // scale down the input aabb's so they are in local (non-scaled) coordinates - 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]); + 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]); // account for local origin localAabbMin += m_localOrigin; localAabbMax += m_localOrigin; //quantize the aabbMin and aabbMax, and adjust the start/end ranges - int quantizedAabbMin[3]; - int quantizedAabbMax[3]; - quantizeWithClamp(quantizedAabbMin, localAabbMin,0); - quantizeWithClamp(quantizedAabbMax, localAabbMax,1); - + int quantizedAabbMin[3]; + int quantizedAabbMax[3]; + quantizeWithClamp(quantizedAabbMin, localAabbMin, 0); + quantizeWithClamp(quantizedAabbMax, localAabbMax, 1); + // expand the min/max quantized values // this is to catch the case where the input aabb falls between grid points! - for (int i = 0; i < 3; ++i) { + for (int i = 0; i < 3; ++i) + { quantizedAabbMin[i]--; quantizedAabbMax[i]++; - } + } - int startX=0; - int endX=m_heightStickWidth-1; - int startJ=0; - int endJ=m_heightStickLength-1; + int startX = 0; + int endX = m_heightStickWidth - 1; + int startJ = 0; + int endJ = m_heightStickLength - 1; switch (m_upAxis) { - case 0: + case 0: { - if (quantizedAabbMin[1]>startX) + if (quantizedAabbMin[1] > startX) startX = quantizedAabbMin[1]; - if (quantizedAabbMax[1]<endX) + if (quantizedAabbMax[1] < endX) endX = quantizedAabbMax[1]; - if (quantizedAabbMin[2]>startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]<endJ) + if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; } - case 1: + case 1: { - if (quantizedAabbMin[0]>startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]<endX) + if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; - if (quantizedAabbMin[2]>startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]<endJ) + if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; }; - case 2: + case 2: { - if (quantizedAabbMin[0]>startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]<endX) + if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; - if (quantizedAabbMin[1]>startJ) + if (quantizedAabbMin[1] > startJ) startJ = quantizedAabbMin[1]; - if (quantizedAabbMax[1]<endJ) + if (quantizedAabbMax[1] < endJ) endJ = quantizedAabbMax[1]; break; } - default: + default: { //need to get valid m_upAxis btAssert(0); } } - - + // TODO If m_vboundsGrid is available, use it to determine if we really need to process this area - for(int j=startJ; j<endJ; j++) + for (int j = startJ; j < endJ; j++) { - for(int x=startX; x<endX; x++) + for (int x = startX; x < endX; x++) { btVector3 vertices[3]; - if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision && !(j & 1))) + int indices[3] = { 0, 1, 2 }; + if (m_flipTriangleWinding) + { + indices[0] = 2; + indices[2] = 0; + } + + if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1))) { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x, j + 1, vertices[1]); - getVertex(x + 1, j + 1, vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman - getVertex(x+1,j+1,vertices[1]); - getVertex(x + 1, j, vertices[2]); - callback->processTriangle(vertices, x, j); - - } else + //first triangle + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j + 1, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x, j); + //second triangle + // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman + getVertex(x + 1, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x+1, j); + } + else { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j,vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - getVertex(x+1,j,vertices[0]); - //getVertex(x,j+1,vertices[1]); - getVertex(x+1,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); + //first triangle + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x, j); + //second triangle + getVertex(x + 1, j, vertices[indices[0]]); + //getVertex(x,j+1,vertices[1]); + getVertex(x + 1, j + 1, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x+1, j); } } } - - - } -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar, btVector3& inertia) const { //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) +void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } @@ -407,3 +394,458 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const { return m_localScaling; } + +namespace +{ + struct GridRaycastState + { + int x; // Next quad coords + int z; + int prev_x; // Previous quad coords + int prev_z; + btScalar param; // Exit param for previous quad + btScalar prevParam; // Enter param for previous quad + btScalar maxDistanceFlat; + btScalar maxDistance3d; + }; +} + +// TODO Does it really need to take 3D vectors? +/// Iterates through a virtual 2D grid of unit-sized square cells, +/// and executes an action on each cell intersecting the given segment, ordered from begin to end. +/// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf +template <typename Action_T> +void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos, int indices[3]) +{ + GridRaycastState rs; + rs.maxDistance3d = beginPos.distance(endPos); + if (rs.maxDistance3d < 0.0001) + { + // Consider the ray is too small to hit anything + return; + } + + + btScalar rayDirectionFlatX = endPos[indices[0]] - beginPos[indices[0]]; + btScalar rayDirectionFlatZ = endPos[indices[2]] - beginPos[indices[2]]; + rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ); + + if (rs.maxDistanceFlat < 0.0001) + { + // Consider the ray vertical + rayDirectionFlatX = 0; + rayDirectionFlatZ = 0; + } + else + { + rayDirectionFlatX /= rs.maxDistanceFlat; + rayDirectionFlatZ /= rs.maxDistanceFlat; + } + + const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0; + const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0; + + const float infinite = 9999999; + const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite; + const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite; + + // pos = param * dir + btScalar paramCrossX; // At which value of `param` we will cross a x-axis lane? + btScalar paramCrossZ; // At which value of `param` we will cross a z-axis lane? + + // paramCrossX and paramCrossZ are initialized as being the first cross + // X initialization + if (xiStep != 0) + { + if (xiStep == 1) + { + paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX; + } + else + { + paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX; + } + } + else + { + paramCrossX = infinite; // Will never cross on X + } + + // Z initialization + if (ziStep != 0) + { + if (ziStep == 1) + { + paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ; + } + else + { + paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ; + } + } + else + { + paramCrossZ = infinite; // Will never cross on Z + } + + rs.x = static_cast<int>(floor(beginPos[indices[0]])); + rs.z = static_cast<int>(floor(beginPos[indices[2]])); + + // Workaround cases where the ray starts at an integer position + if (paramCrossX == 0.0) + { + paramCrossX += paramDeltaX; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction + if (xiStep == -1) + { + rs.x -= 1; + } + } + + if (paramCrossZ == 0.0) + { + paramCrossZ += paramDeltaZ; + if (ziStep == -1) + rs.z -= 1; + } + + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.param = 0; + + while (true) + { + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.prevParam = rs.param; + + if (paramCrossX < paramCrossZ) + { + // X lane + rs.x += xiStep; + // Assign before advancing the param, + // to be in sync with the initialization step + rs.param = paramCrossX; + paramCrossX += paramDeltaX; + } + else + { + // Z lane + rs.z += ziStep; + rs.param = paramCrossZ; + paramCrossZ += paramDeltaZ; + } + + if (rs.param > rs.maxDistanceFlat) + { + rs.param = rs.maxDistanceFlat; + quadAction(rs); + break; + } + else + { + quadAction(rs); + } + } +} + +struct ProcessTrianglesAction +{ + const btHeightfieldTerrainShape* shape; + bool flipQuadEdges; + bool useDiamondSubdivision; + int width; + int length; + btTriangleCallback* callback; + + void exec(int x, int z) const + { + if (x < 0 || z < 0 || x >= width || z >= length) + { + return; + } + + btVector3 vertices[3]; + + // TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit + + // Check quad + if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0))) + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z + 1, vertices[1]); + shape->getVertex(x, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + else + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x + 1, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + } + + void operator()(const GridRaycastState& bs) const + { + exec(bs.prev_x, bs.prev_z); + } +}; + +struct ProcessVBoundsAction +{ + const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& vbounds; + int width; + int length; + int chunkSize; + + btVector3 rayBegin; + btVector3 rayEnd; + btVector3 rayDir; + + int* m_indices; + ProcessTrianglesAction processTriangles; + + ProcessVBoundsAction(const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& bnd, int* indices) + : vbounds(bnd), + m_indices(indices) + { + } + void operator()(const GridRaycastState& rs) const + { + int x = rs.prev_x; + int z = rs.prev_z; + + if (x < 0 || z < 0 || x >= width || z >= length) + { + return; + } + + const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width]; + + btVector3 enterPos; + btVector3 exitPos; + + if (rs.maxDistanceFlat > 0.0001) + { + btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat; + btScalar enterParam3d = rs.prevParam * flatTo3d; + btScalar exitParam3d = rs.param * flatTo3d; + enterPos = rayBegin + rayDir * enterParam3d; + exitPos = rayBegin + rayDir * exitParam3d; + + // We did enter the flat projection of the AABB, + // but we have to check if we intersect it on the vertical axis + if (enterPos[1] > chunk.max && exitPos[m_indices[1]] > chunk.max) + { + return; + } + if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min) + { + return; + } + } + else + { + // Consider the ray vertical + // (though we shouldn't reach this often because there is an early check up-front) + enterPos = rayBegin; + exitPos = rayEnd; + } + + gridRaycast(processTriangles, enterPos, exitPos, m_indices); + // Note: it could be possible to have more than one grid at different levels, + // to do this there would be a branch using a pointer to another ProcessVBoundsAction + } +}; + +// TODO How do I interrupt the ray when there is a hit? `callback` does not return any result +/// Performs a raycast using a hierarchical Bresenham algorithm. +/// Does not allocate any memory by itself. +void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const +{ + // Transform to cell-local + btVector3 beginPos = raySource / m_localScaling; + btVector3 endPos = rayTarget / m_localScaling; + beginPos += m_localOrigin; + endPos += m_localOrigin; + + ProcessTrianglesAction processTriangles; + processTriangles.shape = this; + processTriangles.flipQuadEdges = m_flipQuadEdges; + processTriangles.useDiamondSubdivision = m_useDiamondSubdivision; + processTriangles.callback = callback; + processTriangles.width = m_heightStickWidth - 1; + processTriangles.length = m_heightStickLength - 1; + + // TODO Transform vectors to account for m_upAxis + int indices[3] = { 0, 1, 2 }; + if (m_upAxis == 2) + { + indices[1] = 2; + indices[2] = 1; + } + int iBeginX = static_cast<int>(floor(beginPos[indices[0]])); + int iBeginZ = static_cast<int>(floor(beginPos[indices[2]])); + int iEndX = static_cast<int>(floor(endPos[indices[0]])); + int iEndZ = static_cast<int>(floor(endPos[indices[2]])); + + if (iBeginX == iEndX && iBeginZ == iEndZ) + { + // The ray will never cross quads within the plane, + // so directly process triangles within one quad + // (typically, vertical rays should end up here) + processTriangles.exec(iBeginX, iEndZ); + return; + } + + + + if (m_vboundsGrid.size()==0) + { + // Process all quads intersecting the flat projection of the ray + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); + } + else + { + btVector3 rayDiff = endPos - beginPos; + btScalar flatDistance2 = rayDiff[indices[0]] * rayDiff[indices[0]] + rayDiff[indices[2]] * rayDiff[indices[2]]; + if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize) + { + // Don't use chunks, the ray is too short in the plane + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); + } + + ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]); + processVBounds.width = m_vboundsGridWidth; + processVBounds.length = m_vboundsGridLength; + processVBounds.rayBegin = beginPos; + processVBounds.rayEnd = endPos; + processVBounds.rayDir = rayDiff.normalized(); + processVBounds.processTriangles = processTriangles; + processVBounds.chunkSize = m_vboundsChunkSize; + // The ray is long, run raycast on a higher-level grid + gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize, indices); + } +} + +/// Builds a grid data structure storing the min and max heights of the terrain in chunks. +/// if chunkSize is zero, that accelerator is removed. +/// If you modify the heights, you need to rebuild this accelerator. +void btHeightfieldTerrainShape::buildAccelerator(int chunkSize) +{ + if (chunkSize <= 0) + { + clearAccelerator(); + return; + } + + m_vboundsChunkSize = chunkSize; + int nChunksX = m_heightStickWidth / chunkSize; + int nChunksZ = m_heightStickLength / chunkSize; + + if (m_heightStickWidth % chunkSize > 0) + { + ++nChunksX; // In case terrain size isn't dividable by chunk size + } + if (m_heightStickLength % chunkSize > 0) + { + ++nChunksZ; + } + + if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ) + { + clearAccelerator(); + m_vboundsGridWidth = nChunksX; + m_vboundsGridLength = nChunksZ; + } + + if (nChunksX == 0 || nChunksZ == 0) + { + return; + } + + // This data structure is only reallocated if the required size changed + m_vboundsGrid.resize(nChunksX * nChunksZ); + + // Compute min and max height for all chunks + for (int cz = 0; cz < nChunksZ; ++cz) + { + int z0 = cz * chunkSize; + + for (int cx = 0; cx < nChunksX; ++cx) + { + int x0 = cx * chunkSize; + + Range r; + + r.min = getRawHeightFieldValue(x0, z0); + r.max = r.min; + + // Compute min and max height for this chunk. + // We have to include one extra cell to account for neighbors. + // Here is why: + // Say we have a flat terrain, and a plateau that fits a chunk perfectly. + // + // Left Right + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // x + // + // If the AABB for the Left chunk did not share vertices with the Right, + // then we would fail collision tests at x due to a gap. + // + for (int z = z0; z < z0 + chunkSize + 1; ++z) + { + if (z >= m_heightStickLength) + { + continue; + } + + for (int x = x0; x < x0 + chunkSize + 1; ++x) + { + if (x >= m_heightStickWidth) + { + continue; + } + + btScalar height = getRawHeightFieldValue(x, z); + + if (height < r.min) + { + r.min = height; + } + else if (height > r.max) + { + r.max = height; + } + } + } + + m_vboundsGrid[cx + cz * nChunksX] = r; + } + } +} + +void btHeightfieldTerrainShape::clearAccelerator() +{ + m_vboundsGrid.clear(); +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 4a7a4a4bda4..8dea98fc6be 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_HEIGHTFIELD_TERRAIN_SHAPE_H #include "btConcaveShape.h" +#include "LinearMath/btAlignedObjectArray.h" ///btHeightfieldTerrainShape simulates a 2D heightfield terrain /** @@ -68,43 +69,58 @@ subject to the following restrictions: For usage and testing see the TerrainDemo. */ -ATTRIBUTE_ALIGNED16(class) btHeightfieldTerrainShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btHeightfieldTerrainShape : public btConcaveShape { +public: + struct Range + { + btScalar min; + btScalar max; + }; + protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - btVector3 m_localOrigin; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + btVector3 m_localOrigin; ///terrain data - int m_heightStickWidth; + int m_heightStickWidth; int m_heightStickLength; - btScalar m_minHeight; - btScalar m_maxHeight; + btScalar m_minHeight; + btScalar m_maxHeight; btScalar m_width; btScalar m_length; btScalar m_heightScale; - union - { - const unsigned char* m_heightfieldDataUnsignedChar; - const short* m_heightfieldDataShort; - const btScalar* m_heightfieldDataFloat; - const void* m_heightfieldDataUnknown; + union { + const unsigned char* m_heightfieldDataUnsignedChar; + const short* m_heightfieldDataShort; + const btScalar* m_heightfieldDataFloat; + const void* m_heightfieldDataUnknown; }; - PHY_ScalarType m_heightDataType; - bool m_flipQuadEdges; - bool m_useDiamondSubdivision; + PHY_ScalarType m_heightDataType; + bool m_flipQuadEdges; + bool m_useDiamondSubdivision; bool m_useZigzagSubdivision; + bool m_flipTriangleWinding; + int m_upAxis; - int m_upAxis; - - btVector3 m_localScaling; + btVector3 m_localScaling; + + // Accelerator + btAlignedObjectArray<Range> m_vboundsGrid; + int m_vboundsGridWidth; + int m_vboundsGridLength; + int m_vboundsChunkSize; - virtual btScalar getRawHeightFieldValue(int x,int y) const; - void quantizeWithClamp(int* out, const btVector3& point,int isMax) const; - void getVertex(int x,int y,btVector3& vertex) const; + int m_userIndex2; + btScalar m_userValue3; + struct btTriangleInfoMap* m_triangleInfoMap; + virtual btScalar getRawHeightFieldValue(int x, int y) const; + void quantizeWithClamp(int* out, const btVector3& point, int isMax) const; /// protected initialization /** @@ -112,25 +128,24 @@ protected: backwards-compatible without a lot of copy/paste. */ void initialize(int heightStickWidth, int heightStickLength, - const void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, int upAxis, - PHY_ScalarType heightDataType, bool flipQuadEdges); + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType heightDataType, bool flipQuadEdges); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - + /// preferred constructor /** This constructor supports a range of heightfield data types, and allows for a non-zero minimum height value. heightScale is needed for any integer-based heightfield data types. */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, - const void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, - int upAxis, PHY_ScalarType heightDataType, - bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, PHY_ScalarType heightDataType, + bool flipQuadEdges); /// legacy constructor /** @@ -139,29 +154,76 @@ public: compatibility reasons, heightScale is calculated as maxHeight / 65535 (and is only used when useFloatData = false). */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,const void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); + void setUseDiamondSubdivision(bool useDiamondSubdivision = true) { m_useDiamondSubdivision = useDiamondSubdivision; } - void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} + ///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625 + void setUseZigzagSubdivision(bool useZigzagSubdivision = true) { m_useZigzagSubdivision = useZigzagSubdivision; } - ///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625 - void setUseZigzagSubdivision(bool useZigzagSubdivision=true) { m_useZigzagSubdivision = useZigzagSubdivision;} + void setFlipTriangleWinding(bool flipTriangleWinding) + { + m_flipTriangleWinding = flipTriangleWinding; + } + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - 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 processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void setLocalScaling(const btVector3& scaling); - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const; - + + void getVertex(int x, int y, btVector3& vertex) const; + + void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget) const; + + void buildAccelerator(int chunkSize = 16); + void clearAccelerator(); + + int getUpAxis() const + { + return m_upAxis; + } //debugging - virtual const char* getName()const {return "HEIGHTFIELD";} + virtual const char* getName() const { return "HEIGHTFIELD"; } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + int getUserIndex2() const + { + return m_userIndex2; + } + void setUserValue3(btScalar value) + { + m_userValue3 = value; + } + btScalar getUserValue3() const + { + return m_userValue3; + } + const struct btTriangleInfoMap* getTriangleInfoMap() const + { + return m_triangleInfoMap; + } + struct btTriangleInfoMap* getTriangleInfoMap() + { + return m_triangleInfoMap; + } + void setTriangleInfoMap(btTriangleInfoMap* map) + { + m_triangleInfoMap = map; + } + const unsigned char* getHeightfieldRawData() const + { + return m_heightfieldDataUnsignedChar; + } }; -#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h index 866f9b4da4d..c9a436bf26a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h @@ -21,15 +21,18 @@ subject to the following restrictions: // 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 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; } + btScalar m_friction; + btScalar m_restitution; + int pad[2]; + + btMaterial() {} + btMaterial(btScalar fric, btScalar rest) + { + m_friction = fric; + m_restitution = rest; + } }; -#endif // BT_MATERIAL_H - +#endif // BT_MATERIAL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp new file mode 100644 index 00000000000..13c0a343f17 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp @@ -0,0 +1,522 @@ +#include "btMiniSDF.h" + +// +//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid +//example: +//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj +//The MIT License (MIT) +// +//Copyright (c) 2017 Dan Koschier +// + +#include <limits.h> +#include <string.h> //memcpy + +struct btSdfDataStream +{ + const char* m_data; + int m_size; + + int m_currentOffset; + + btSdfDataStream(const char* data, int size) + : m_data(data), + m_size(size), + m_currentOffset(0) + { + } + + template <class T> + bool read(T& val) + { + int bytes = sizeof(T); + if (m_currentOffset + bytes <= m_size) + { + char* dest = (char*)&val; + memcpy(dest, &m_data[m_currentOffset], bytes); + m_currentOffset += bytes; + return true; + } + btAssert(0); + return false; + } +}; + +bool btMiniSDF::load(const char* data, int size) +{ + int fileSize = -1; + + btSdfDataStream ds(data, size); + { + double buf[6]; + ds.read(buf); + m_domain.m_min[0] = buf[0]; + m_domain.m_min[1] = buf[1]; + m_domain.m_min[2] = buf[2]; + m_domain.m_min[3] = 0; + m_domain.m_max[0] = buf[3]; + m_domain.m_max[1] = buf[4]; + m_domain.m_max[2] = buf[5]; + m_domain.m_max[3] = 0; + } + { + unsigned int buf2[3]; + ds.read(buf2); + m_resolution[0] = buf2[0]; + m_resolution[1] = buf2[1]; + m_resolution[2] = buf2[2]; + } + { + double buf[3]; + ds.read(buf); + m_cell_size[0] = buf[0]; + m_cell_size[1] = buf[1]; + m_cell_size[2] = buf[2]; + } + { + double buf[3]; + ds.read(buf); + m_inv_cell_size[0] = buf[0]; + m_inv_cell_size[1] = buf[1]; + m_inv_cell_size[2] = buf[2]; + } + { + unsigned long long int cells; + ds.read(cells); + m_n_cells = cells; + } + { + unsigned long long int fields; + ds.read(fields); + m_n_fields = fields; + } + + unsigned long long int nodes0; + std::size_t n_nodes0; + ds.read(nodes0); + n_nodes0 = nodes0; + if (n_nodes0 > 1024 * 1024 * 1024) + { + return m_isValid; + } + m_nodes.resize(n_nodes0); + for (unsigned int i = 0; i < n_nodes0; i++) + { + unsigned long long int n_nodes1; + ds.read(n_nodes1); + btAlignedObjectArray<double>& nodes = m_nodes[i]; + nodes.resize(n_nodes1); + for (int j = 0; j < nodes.size(); j++) + { + double& node = nodes[j]; + ds.read(node); + } + } + + unsigned long long int n_cells0; + ds.read(n_cells0); + m_cells.resize(n_cells0); + for (int i = 0; i < n_cells0; i++) + { + unsigned long long int n_cells1; + btAlignedObjectArray<btCell32>& cells = m_cells[i]; + ds.read(n_cells1); + cells.resize(n_cells1); + for (int j = 0; j < n_cells1; j++) + { + btCell32& cell = cells[j]; + ds.read(cell); + } + } + + { + unsigned long long int n_cell_maps0; + ds.read(n_cell_maps0); + + m_cell_map.resize(n_cell_maps0); + for (int i = 0; i < n_cell_maps0; i++) + { + unsigned long long int n_cell_maps1; + btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i]; + ds.read(n_cell_maps1); + cell_maps.resize(n_cell_maps1); + for (int j = 0; j < n_cell_maps1; j++) + { + unsigned int& cell_map = cell_maps[j]; + ds.read(cell_map); + } + } + } + + m_isValid = (ds.m_currentOffset == ds.m_size); + return m_isValid; +} + +unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const& ijk) const +{ + return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0]; +} + +btAlignedBox3d +btMiniSDF::subdomain(btMultiIndex const& ijk) const +{ + btAssert(m_isValid); + btVector3 tmp; + tmp.m_floats[0] = m_cell_size[0] * (double)ijk.ijk[0]; + tmp.m_floats[1] = m_cell_size[1] * (double)ijk.ijk[1]; + tmp.m_floats[2] = m_cell_size[2] * (double)ijk.ijk[2]; + + btVector3 origin = m_domain.min() + tmp; + + btAlignedBox3d box = btAlignedBox3d(origin, origin + m_cell_size); + return box; +} + +btMultiIndex +btMiniSDF::singleToMultiIndex(unsigned int l) const +{ + btAssert(m_isValid); + unsigned int n01 = m_resolution[0] * m_resolution[1]; + unsigned int k = l / n01; + unsigned int temp = l % n01; + unsigned int j = temp / m_resolution[0]; + unsigned int i = temp % m_resolution[0]; + btMultiIndex mi; + mi.ijk[0] = i; + mi.ijk[1] = j; + mi.ijk[2] = k; + return mi; +} + +btAlignedBox3d +btMiniSDF::subdomain(unsigned int l) const +{ + btAssert(m_isValid); + return subdomain(singleToMultiIndex(l)); +} + +btShapeMatrix +btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const +{ + btAssert(m_isValid); + btShapeMatrix res; + + btScalar x = xi[0]; + btScalar y = xi[1]; + btScalar z = xi[2]; + + btScalar x2 = x * x; + btScalar y2 = y * y; + btScalar z2 = z * z; + + btScalar _1mx = 1.0 - x; + btScalar _1my = 1.0 - y; + btScalar _1mz = 1.0 - z; + + btScalar _1px = 1.0 + x; + btScalar _1py = 1.0 + y; + btScalar _1pz = 1.0 + z; + + btScalar _1m3x = 1.0 - 3.0 * x; + btScalar _1m3y = 1.0 - 3.0 * y; + btScalar _1m3z = 1.0 - 3.0 * z; + + btScalar _1p3x = 1.0 + 3.0 * x; + btScalar _1p3y = 1.0 + 3.0 * y; + btScalar _1p3z = 1.0 + 3.0 * z; + + btScalar _1mxt1my = _1mx * _1my; + btScalar _1mxt1py = _1mx * _1py; + btScalar _1pxt1my = _1px * _1my; + btScalar _1pxt1py = _1px * _1py; + + btScalar _1mxt1mz = _1mx * _1mz; + btScalar _1mxt1pz = _1mx * _1pz; + btScalar _1pxt1mz = _1px * _1mz; + btScalar _1pxt1pz = _1px * _1pz; + + btScalar _1myt1mz = _1my * _1mz; + btScalar _1myt1pz = _1my * _1pz; + btScalar _1pyt1mz = _1py * _1mz; + btScalar _1pyt1pz = _1py * _1pz; + + btScalar _1mx2 = 1.0 - x2; + btScalar _1my2 = 1.0 - y2; + btScalar _1mz2 = 1.0 - z2; + + // Corner nodes. + btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0); + res[0] = fac * _1mxt1my * _1mz; + res[1] = fac * _1pxt1my * _1mz; + res[2] = fac * _1mxt1py * _1mz; + res[3] = fac * _1pxt1py * _1mz; + res[4] = fac * _1mxt1my * _1pz; + res[5] = fac * _1pxt1my * _1pz; + res[6] = fac * _1mxt1py * _1pz; + res[7] = fac * _1pxt1py * _1pz; + + // Edge nodes. + + fac = 9.0 / 64.0 * _1mx2; + btScalar fact1m3x = fac * _1m3x; + btScalar fact1p3x = fac * _1p3x; + res[8] = fact1m3x * _1myt1mz; + res[9] = fact1p3x * _1myt1mz; + res[10] = fact1m3x * _1myt1pz; + res[11] = fact1p3x * _1myt1pz; + res[12] = fact1m3x * _1pyt1mz; + res[13] = fact1p3x * _1pyt1mz; + res[14] = fact1m3x * _1pyt1pz; + res[15] = fact1p3x * _1pyt1pz; + + fac = 9.0 / 64.0 * _1my2; + btScalar fact1m3y = fac * _1m3y; + btScalar fact1p3y = fac * _1p3y; + res[16] = fact1m3y * _1mxt1mz; + res[17] = fact1p3y * _1mxt1mz; + res[18] = fact1m3y * _1pxt1mz; + res[19] = fact1p3y * _1pxt1mz; + res[20] = fact1m3y * _1mxt1pz; + res[21] = fact1p3y * _1mxt1pz; + res[22] = fact1m3y * _1pxt1pz; + res[23] = fact1p3y * _1pxt1pz; + + fac = 9.0 / 64.0 * _1mz2; + btScalar fact1m3z = fac * _1m3z; + btScalar fact1p3z = fac * _1p3z; + res[24] = fact1m3z * _1mxt1my; + res[25] = fact1p3z * _1mxt1my; + res[26] = fact1m3z * _1mxt1py; + res[27] = fact1p3z * _1mxt1py; + res[28] = fact1m3z * _1pxt1my; + res[29] = fact1p3z * _1pxt1my; + res[30] = fact1m3z * _1pxt1py; + res[31] = fact1p3z * _1pxt1py; + + if (gradient) + { + btShapeGradients& dN = *gradient; + + btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0; + btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0; + btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0; + btScalar _18x = 18.0 * x; + btScalar _18y = 18.0 * y; + btScalar _18z = 18.0 * z; + + btScalar _3m9x2 = 3.0 - 9.0 * x2; + btScalar _3m9y2 = 3.0 - 9.0 * y2; + btScalar _3m9z2 = 3.0 - 9.0 * z2; + + btScalar _2x = 2.0 * x; + btScalar _2y = 2.0 * y; + btScalar _2z = 2.0 * z; + + btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19; + btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19; + btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19; + btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19; + btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19; + btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19; + + dN(0, 0) = _18xm9t3x2py2pz2m19 * _1myt1mz; + dN(0, 1) = _1mxt1mz * _18ym9tx2p3y2pz2m19; + dN(0, 2) = _1mxt1my * _18zm9tx2py2p3z2m19; + dN(1, 0) = _18xp9t3x2py2pz2m19 * _1myt1mz; + dN(1, 1) = _1pxt1mz * _18ym9tx2p3y2pz2m19; + dN(1, 2) = _1pxt1my * _18zm9tx2py2p3z2m19; + dN(2, 0) = _18xm9t3x2py2pz2m19 * _1pyt1mz; + dN(2, 1) = _1mxt1mz * _18yp9tx2p3y2pz2m19; + dN(2, 2) = _1mxt1py * _18zm9tx2py2p3z2m19; + dN(3, 0) = _18xp9t3x2py2pz2m19 * _1pyt1mz; + dN(3, 1) = _1pxt1mz * _18yp9tx2p3y2pz2m19; + dN(3, 2) = _1pxt1py * _18zm9tx2py2p3z2m19; + dN(4, 0) = _18xm9t3x2py2pz2m19 * _1myt1pz; + dN(4, 1) = _1mxt1pz * _18ym9tx2p3y2pz2m19; + dN(4, 2) = _1mxt1my * _18zp9tx2py2p3z2m19; + dN(5, 0) = _18xp9t3x2py2pz2m19 * _1myt1pz; + dN(5, 1) = _1pxt1pz * _18ym9tx2p3y2pz2m19; + dN(5, 2) = _1pxt1my * _18zp9tx2py2p3z2m19; + dN(6, 0) = _18xm9t3x2py2pz2m19 * _1pyt1pz; + dN(6, 1) = _1mxt1pz * _18yp9tx2p3y2pz2m19; + dN(6, 2) = _1mxt1py * _18zp9tx2py2p3z2m19; + dN(7, 0) = _18xp9t3x2py2pz2m19 * _1pyt1pz; + dN(7, 1) = _1pxt1pz * _18yp9tx2p3y2pz2m19; + dN(7, 2) = _1pxt1py * _18zp9tx2py2p3z2m19; + + dN.topRowsDivide(8, 64.0); + + btScalar _m3m9x2m2x = -_3m9x2 - _2x; + btScalar _p3m9x2m2x = _3m9x2 - _2x; + btScalar _1mx2t1m3x = _1mx2 * _1m3x; + btScalar _1mx2t1p3x = _1mx2 * _1p3x; + dN(8, 0) = _m3m9x2m2x * _1myt1mz, + dN(8, 1) = -_1mx2t1m3x * _1mz, + dN(8, 2) = -_1mx2t1m3x * _1my; + dN(9, 0) = _p3m9x2m2x * _1myt1mz, + dN(9, 1) = -_1mx2t1p3x * _1mz, + dN(9, 2) = -_1mx2t1p3x * _1my; + dN(10, 0) = _m3m9x2m2x * _1myt1pz, + dN(10, 1) = -_1mx2t1m3x * _1pz, + dN(10, 2) = _1mx2t1m3x * _1my; + dN(11, 0) = _p3m9x2m2x * _1myt1pz, + dN(11, 1) = -_1mx2t1p3x * _1pz, + dN(11, 2) = _1mx2t1p3x * _1my; + dN(12, 0) = _m3m9x2m2x * _1pyt1mz, + dN(12, 1) = _1mx2t1m3x * _1mz, + dN(12, 2) = -_1mx2t1m3x * _1py; + dN(13, 0) = _p3m9x2m2x * _1pyt1mz, + dN(13, 1) = _1mx2t1p3x * _1mz, + dN(13, 2) = -_1mx2t1p3x * _1py; + dN(14, 0) = _m3m9x2m2x * _1pyt1pz, + dN(14, 1) = _1mx2t1m3x * _1pz, + dN(14, 2) = _1mx2t1m3x * _1py; + dN(15, 0) = _p3m9x2m2x * _1pyt1pz, + dN(15, 1) = _1mx2t1p3x * _1pz, + dN(15, 2) = _1mx2t1p3x * _1py; + + btScalar _m3m9y2m2y = -_3m9y2 - _2y; + btScalar _p3m9y2m2y = _3m9y2 - _2y; + btScalar _1my2t1m3y = _1my2 * _1m3y; + btScalar _1my2t1p3y = _1my2 * _1p3y; + dN(16, 0) = -_1my2t1m3y * _1mz, + dN(16, 1) = _m3m9y2m2y * _1mxt1mz, + dN(16, 2) = -_1my2t1m3y * _1mx; + dN(17, 0) = -_1my2t1p3y * _1mz, + dN(17, 1) = _p3m9y2m2y * _1mxt1mz, + dN(17, 2) = -_1my2t1p3y * _1mx; + dN(18, 0) = _1my2t1m3y * _1mz, + dN(18, 1) = _m3m9y2m2y * _1pxt1mz, + dN(18, 2) = -_1my2t1m3y * _1px; + dN(19, 0) = _1my2t1p3y * _1mz, + dN(19, 1) = _p3m9y2m2y * _1pxt1mz, + dN(19, 2) = -_1my2t1p3y * _1px; + dN(20, 0) = -_1my2t1m3y * _1pz, + dN(20, 1) = _m3m9y2m2y * _1mxt1pz, + dN(20, 2) = _1my2t1m3y * _1mx; + dN(21, 0) = -_1my2t1p3y * _1pz, + dN(21, 1) = _p3m9y2m2y * _1mxt1pz, + dN(21, 2) = _1my2t1p3y * _1mx; + dN(22, 0) = _1my2t1m3y * _1pz, + dN(22, 1) = _m3m9y2m2y * _1pxt1pz, + dN(22, 2) = _1my2t1m3y * _1px; + dN(23, 0) = _1my2t1p3y * _1pz, + dN(23, 1) = _p3m9y2m2y * _1pxt1pz, + dN(23, 2) = _1my2t1p3y * _1px; + + btScalar _m3m9z2m2z = -_3m9z2 - _2z; + btScalar _p3m9z2m2z = _3m9z2 - _2z; + btScalar _1mz2t1m3z = _1mz2 * _1m3z; + btScalar _1mz2t1p3z = _1mz2 * _1p3z; + dN(24, 0) = -_1mz2t1m3z * _1my, + dN(24, 1) = -_1mz2t1m3z * _1mx, + dN(24, 2) = _m3m9z2m2z * _1mxt1my; + dN(25, 0) = -_1mz2t1p3z * _1my, + dN(25, 1) = -_1mz2t1p3z * _1mx, + dN(25, 2) = _p3m9z2m2z * _1mxt1my; + dN(26, 0) = -_1mz2t1m3z * _1py, + dN(26, 1) = _1mz2t1m3z * _1mx, + dN(26, 2) = _m3m9z2m2z * _1mxt1py; + dN(27, 0) = -_1mz2t1p3z * _1py, + dN(27, 1) = _1mz2t1p3z * _1mx, + dN(27, 2) = _p3m9z2m2z * _1mxt1py; + dN(28, 0) = _1mz2t1m3z * _1my, + dN(28, 1) = -_1mz2t1m3z * _1px, + dN(28, 2) = _m3m9z2m2z * _1pxt1my; + dN(29, 0) = _1mz2t1p3z * _1my, + dN(29, 1) = -_1mz2t1p3z * _1px, + dN(29, 2) = _p3m9z2m2z * _1pxt1my; + dN(30, 0) = _1mz2t1m3z * _1py, + dN(30, 1) = _1mz2t1m3z * _1px, + dN(30, 2) = _m3m9z2m2z * _1pxt1py; + dN(31, 0) = _1mz2t1p3z * _1py, + dN(31, 1) = _1mz2t1p3z * _1px, + dN(31, 2) = _p3m9z2m2z * _1pxt1py; + + dN.bottomRowsMul(32u - 8u, 9.0 / 64.0); + } + + return res; +} + +bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x, + btVector3* gradient) const +{ + btAssert(m_isValid); + if (!m_isValid) + return false; + + if (!m_domain.contains(x)) + return false; + + btVector3 tmpmi = ((x - m_domain.min()) * (m_inv_cell_size)); //.cast<unsigned int>().eval(); + unsigned int mi[3] = {(unsigned int)tmpmi[0], (unsigned int)tmpmi[1], (unsigned int)tmpmi[2]}; + if (mi[0] >= m_resolution[0]) + mi[0] = m_resolution[0] - 1; + if (mi[1] >= m_resolution[1]) + mi[1] = m_resolution[1] - 1; + if (mi[2] >= m_resolution[2]) + mi[2] = m_resolution[2] - 1; + btMultiIndex mui; + mui.ijk[0] = mi[0]; + mui.ijk[1] = mi[1]; + mui.ijk[2] = mi[2]; + int i = multiToSingleIndex(mui); + unsigned int i_ = m_cell_map[field_id][i]; + if (i_ == UINT_MAX) + return false; + + btAlignedBox3d sd = subdomain(i); + i = i_; + btVector3 d = sd.m_max - sd.m_min; //.diagonal().eval(); + + btVector3 denom = (sd.max() - sd.min()); + btVector3 c0 = btVector3(2.0, 2.0, 2.0) / denom; + btVector3 c1 = (sd.max() + sd.min()) / denom; + btVector3 xi = (c0 * x - c1); + + btCell32 const& cell = m_cells[field_id][i]; + if (!gradient) + { + //auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0)); + double phi = 0.0; + btShapeMatrix N = shape_function_(xi, 0); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + return false; + ; + } + phi += c * N[j]; + } + + dist = phi; + return true; + } + + btShapeGradients dN; + btShapeMatrix N = shape_function_(xi, &dN); + + double phi = 0.0; + gradient->setZero(); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + gradient->setZero(); + return false; + } + phi += c * N[j]; + (*gradient)[0] += c * dN(j, 0); + (*gradient)[1] += c * dN(j, 1); + (*gradient)[2] += c * dN(j, 2); + } + (*gradient) *= c0; + dist = phi; + return true; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h new file mode 100644 index 00000000000..b60fd102fdc --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h @@ -0,0 +1,127 @@ +#ifndef MINISDF_H +#define MINISDF_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btAlignedObjectArray.h" + +struct btMultiIndex +{ + unsigned int ijk[3]; +}; + +struct btAlignedBox3d +{ + btVector3 m_min; + btVector3 m_max; + + const btVector3& min() const + { + return m_min; + } + + const btVector3& max() const + { + return m_max; + } + + bool contains(const btVector3& x) const + { + return TestPointAgainstAabb2(m_min, m_max, x); + } + + btAlignedBox3d(const btVector3& mn, const btVector3& mx) + : m_min(mn), + m_max(mx) + { + } + + btAlignedBox3d() + { + } +}; + +struct btShapeMatrix +{ + double m_vec[32]; + + inline double& operator[](int i) + { + return m_vec[i]; + } + + inline const double& operator[](int i) const + { + return m_vec[i]; + } +}; + +struct btShapeGradients +{ + btVector3 m_vec[32]; + + void topRowsDivide(int row, double denom) + { + for (int i = 0; i < row; i++) + { + m_vec[i] /= denom; + } + } + + void bottomRowsMul(int row, double val) + { + for (int i = 32 - row; i < 32; i++) + { + m_vec[i] *= val; + } + } + + inline btScalar& operator()(int i, int j) + { + return m_vec[i][j]; + } +}; + +struct btCell32 +{ + unsigned int m_cells[32]; +}; + +struct btMiniSDF +{ + btAlignedBox3d m_domain; + unsigned int m_resolution[3]; + btVector3 m_cell_size; + btVector3 m_inv_cell_size; + std::size_t m_n_cells; + std::size_t m_n_fields; + bool m_isValid; + + btAlignedObjectArray<btAlignedObjectArray<double> > m_nodes; + btAlignedObjectArray<btAlignedObjectArray<btCell32> > m_cells; + btAlignedObjectArray<btAlignedObjectArray<unsigned int> > m_cell_map; + + btMiniSDF() + : m_isValid(false) + { + } + bool load(const char* data, int size); + bool isValid() const + { + return m_isValid; + } + unsigned int multiToSingleIndex(btMultiIndex const& ijk) const; + + btAlignedBox3d subdomain(btMultiIndex const& ijk) const; + + btMultiIndex singleToMultiIndex(unsigned int l) const; + + btAlignedBox3d subdomain(unsigned int l) const; + + btShapeMatrix + shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const; + + bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const; +}; + +#endif //MINISDF_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 06707e24e55..d4b6a651de0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -13,48 +13,59 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btMinkowskiSumShape.h" - -btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB) -: btConvexInternalShape (), -m_shapeA(shapeA), -m_shapeB(shapeB) +btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB) + : btConvexInternalShape(), + m_shapeA(shapeA), + m_shapeB(shapeB) { m_shapeType = MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE; m_transA.setIdentity(); m_transB.setIdentity(); } -btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - 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; + 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; } -void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { ///@todo: could make recursive use of batching. probably this shape is not used frequently. - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]); } - } - - -btScalar btMinkowskiSumShape::getMargin() const +btScalar btMinkowskiSumShape::getMargin() const { return m_shapeA->getMargin() + m_shapeB->getMargin(); } - -void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btMinkowskiSumShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; - btAssert(0); - inertia.setValue(0,0,0); + //inertia of the AABB of the Minkowski sum + btTransform identity; + identity.setIdentity(); + btVector3 aabbMin, aabbMax; + getAabb(identity, aabbMin, aabbMax); + + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); + + btScalar margin = getMargin(); + + btScalar lx = btScalar(2.) * (halfExtents.x() + margin); + btScalar ly = btScalar(2.) * (halfExtents.y() + margin); + btScalar lz = btScalar(2.) * (halfExtents.z() + margin); + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; + const btScalar scaledmass = mass * btScalar(0.08333333); + + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index a3f9a472394..3b5150f6d50 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -17,46 +17,43 @@ subject to the following restrictions: #define BT_MINKOWSKI_SUM_SHAPE_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types /// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes. -ATTRIBUTE_ALIGNED16(class) btMinkowskiSumShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btMinkowskiSumShape : public btConvexInternalShape { - - btTransform m_transA; - btTransform m_transB; - const btConvexShape* m_shapeA; - const btConvexShape* m_shapeB; + btTransform m_transA; + btTransform m_transB; + const btConvexShape* m_shapeA; + const btConvexShape* m_shapeB; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB); + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - void setTransformA(const btTransform& transA) { m_transA = transA;} - void setTransformB(const btTransform& transB) { m_transB = transB;} + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - const btTransform& getTransformA()const { return m_transA;} - const btTransform& GetTransformB()const { return m_transB;} + void setTransformA(const btTransform& transA) { m_transA = transA; } + void setTransformB(const btTransform& transB) { m_transB = transB; } + const btTransform& getTransformA() const { return m_transA; } + const btTransform& GetTransformB() const { return m_transB; } - virtual btScalar getMargin() const; + virtual btScalar getMargin() const; - const btConvexShape* getShapeA() const { return m_shapeA;} - const btConvexShape* getShapeB() const { return m_shapeB;} + const btConvexShape* getShapeA() const { return m_shapeA; } + const btConvexShape* getShapeB() const { return m_shapeB; } - virtual const char* getName()const + virtual const char* getName() const { return "MinkowskiSum"; } }; -#endif //BT_MINKOWSKI_SUM_SHAPE_H +#endif //BT_MINKOWSKI_SUM_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index 6abfdffa677..c0cc55dfb08 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -13,52 +13,50 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "btMultiSphereShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" #include "LinearMath/btSerializer.h" -btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres) -:btConvexInternalAabbCachingShape () +btMultiSphereShape::btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres) + : btConvexInternalAabbCachingShape() { m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE; //btScalar startMargin = btScalar(BT_LARGE_FLOAT); m_localPositionArray.resize(numSpheres); m_radiArray.resize(numSpheres); - for (int i=0;i<numSpheres;i++) + for (int i = 0; i < numSpheres; i++) { m_localPositionArray[i] = positions[i]; m_radiArray[i] = radi[i]; - } recalcLocalAabb(); - } #ifndef MIN - #define MIN( _a, _b) ((_a) < (_b) ? (_a) : (_b)) +#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) #endif - btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr < (SIMD_EPSILON * SIMD_EPSILON)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } @@ -69,32 +67,30 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala const btScalar* rad = &m_radiArray[0]; int numSpheres = m_localPositionArray.size(); - for( int k = 0; k < numSpheres; k+= 128 ) + for (int k = 0; k < numSpheres; k += 128) { btVector3 temp[128]; - int inner_count = MIN( numSpheres - k, 128 ); - for( long i = 0; i < inner_count; i++ ) - { - temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); - pos++; - rad++; - } - long i = vec.maxDot( temp, inner_count, newDot); - if( newDot > maxDot ) + int inner_count = MIN(numSpheres - k, 128); + for (long i = 0; i < inner_count; i++) + { + temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin(); + pos++; + rad++; + } + long i = vec.maxDot(temp, inner_count, newDot); + if (newDot > maxDot) { maxDot = newDot; supVec = temp[i]; } - } + } return supVec; - } - void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -107,76 +103,67 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala const btScalar* rad = &m_radiArray[0]; int numSpheres = m_localPositionArray.size(); - for( int k = 0; k < numSpheres; k+= 128 ) - { - btVector3 temp[128]; - int inner_count = MIN( numSpheres - k, 128 ); - for( long i = 0; i < inner_count; i++ ) - { - temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); - pos++; - rad++; - } - long i = vec.maxDot( temp, inner_count, newDot); - if( newDot > maxDot ) - { - maxDot = newDot; - supportVerticesOut[j] = temp[i]; - } - } - + for (int k = 0; k < numSpheres; k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(numSpheres - k, 128); + for (long i = 0; i < inner_count; i++) + { + temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin(); + pos++; + rad++; + } + long i = vec.maxDot(temp, inner_count, newDot); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = temp[i]; + } + } } } - - - - - - - -void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btMultiSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //as an approximation, take the inertia of the box that bounds the spheres - btVector3 localAabbMin,localAabbMax; - getCachedLocalAabb(localAabbMin,localAabbMax); - btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5); - - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); + btVector3 localAabbMin, localAabbMax; + getCachedLocalAabb(localAabbMin, localAabbMax); + btVector3 halfExtents = (localAabbMax - localAabbMin) * btScalar(0.5); - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer; + btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*)dataBuffer; btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); int numElem = m_localPositionArray.size(); - shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]): 0; - + shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]) : 0; + shapeData->m_localPositionArraySize = numElem; if (numElem) { - btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem); + btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius), numElem); btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_localPositionArray[i].serializeFloat(memPtr->m_pos); memPtr->m_radius = float(m_radiArray[i]); } - serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]); + serializer->finalizeChunk(chunk, "btPositionAndRadius", BT_ARRAY_CODE, (void*)&m_localPositionArray[0]); } - - return "btMultiSphereShapeData"; -} + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding)); + return "btMultiSphereShapeData"; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 5d3b4026848..2d79c07ca42 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -17,69 +17,65 @@ subject to the following restrictions: #define BT_MULTI_SPHERE_MINKOWSKI_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAabbUtil2.h" - - ///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, but call 'recalcLocalAabb' after changing any sphere position/radius -ATTRIBUTE_ALIGNED16(class) btMultiSphereShape : public btConvexInternalAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btMultiSphereShape : public btConvexInternalAabbCachingShape { - btAlignedObjectArray<btVector3> m_localPositionArray; - btAlignedObjectArray<btScalar> m_radiArray; - + btAlignedObjectArray<btScalar> m_radiArray; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres); + + btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres); ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - int getSphereCount() const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + int getSphereCount() const { return m_localPositionArray.size(); } - const btVector3& getSpherePosition(int index) const + const btVector3& getSpherePosition(int index) const { return m_localPositionArray[index]; } - btScalar getSphereRadius(int index) const + btScalar getSphereRadius(int index) const { return m_radiArray[index]; } - - virtual const char* getName()const + virtual const char* getName() const { return "MultiSphere"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - -struct btPositionAndRadius +struct btPositionAndRadius { - btVector3FloatData m_pos; - float m_radius; + btVector3FloatData m_pos; + float m_radius; }; +// clang-format off + struct btMultiSphereShapeData { btConvexInternalShapeData m_convexInternalShapeData; @@ -89,13 +85,11 @@ struct btMultiSphereShapeData char m_padding[4]; }; +// clang-format on - -SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const { return sizeof(btMultiSphereShapeData); } - - -#endif //BT_MULTI_SPHERE_MINKOWSKI_H +#endif //BT_MULTI_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp index 58799ac96ad..30108c9e7b4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -19,27 +19,25 @@ subject to the following restrictions: #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 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); + 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 index 5ebaede4a88..d1d42f8e049 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -22,82 +22,80 @@ subject to the following restrictions: #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 +ATTRIBUTE_ALIGNED16(class) +btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape { - btAlignedObjectArray <btMaterial*> m_materialList; + btAlignedObjectArray<btMaterial *> m_materialList; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): - btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - - 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)); - } - } + btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + + 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) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - - 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() - { -/* + btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3 &bvhAabbMin, const btVector3 &bvhAabbMax, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + + 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]); @@ -106,14 +104,12 @@ public: btAlignedFree(m_materialValues); m_materialLookup = NULL; */ - } + } //debugging - virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} - - ///Obtains the material for a specific triangle - const btMaterial * getMaterialProperties(int partID, int triIndex); + virtual const char *getName() const { return "MULTIMATERIALTRIANGLEMESH"; } -} -; + ///Obtains the material for a specific triangle + const btMaterial *getMaterialProperties(int partID, int triIndex); +}; -#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#endif //BT_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 6f36775f7c9..863ea6d6ac2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -13,51 +13,46 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btOptimizedBvh.h" #include "btStridingMeshInterface.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btIDebugDraw.h" - btOptimizedBvh::btOptimizedBvh() -{ +{ } btOptimizedBvh::~btOptimizedBvh() { } - void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax) { m_useQuantization = useQuantizedAabbCompression; - // NodeArray triangleNodes; - struct NodeTriangleCallback : public btInternalTriangleIndexCallback + struct NodeTriangleCallback : public btInternalTriangleIndexCallback { - - NodeArray& m_triangleNodes; + NodeArray& m_triangleNodes; NodeTriangleCallback& operator=(NodeTriangleCallback& other) { m_triangleNodes.copyFromArray(other.m_triangleNodes); return *this; } - - NodeTriangleCallback(NodeArray& triangleNodes) - :m_triangleNodes(triangleNodes) + + NodeTriangleCallback(NodeArray& triangleNodes) + : m_triangleNodes(triangleNodes) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { btOptimizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -70,17 +65,17 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized node.m_aabbMaxOrg = aabbMax; node.m_escapeIndex = -1; - + //for child nodes node.m_subPart = partId; node.m_triangleIndex = triangleIndex; m_triangleNodes.push_back(node); } }; - struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback + struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback { - QuantizedNodeArray& m_triangleNodes; - const btQuantizedBvh* m_optimizedTree; // for quantization + QuantizedNodeArray& m_triangleNodes; + const btQuantizedBvh* m_optimizedTree; // for quantization QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) { @@ -89,23 +84,23 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized return *this; } - QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree) - :m_triangleNodes(triangleNodes),m_optimizedTree(tree) + QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes, const btQuantizedBvh* tree) + : m_triangleNodes(triangleNodes), m_optimizedTree(tree) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { // 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))); + 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); + btAssert(triangleIndex >= 0); btQuantizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -132,59 +127,52 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION); } - m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + 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; + node.m_escapeIndexOrTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_triangleNodes.push_back(node); } }; - - int numLeafNodes = 0; - if (m_useQuantization) { - //initialize quantization values - setQuantizationValues(bvhAabbMin,bvhAabbMax); + setQuantizationValues(bvhAabbMin, bvhAabbMax); - QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); + QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes, this); - - triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); + triangles->InternalProcessAllTriangles(&callback, m_bvhAabbMin, m_bvhAabbMax); //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_quantizedLeafNodes.size(); - - m_quantizedContiguousNodes.resize(2*numLeafNodes); - - - } else + m_quantizedContiguousNodes.resize(2 * numLeafNodes); + } + else { - NodeTriangleCallback callback(m_leafNodes); + NodeTriangleCallback callback(m_leafNodes); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); - triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + triangles->InternalProcessAllTriangles(&callback, aabbMin, aabbMax); //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_leafNodes.size(); - m_contiguousNodes.resize(2*numLeafNodes); + m_contiguousNodes.resize(2 * numLeafNodes); } m_curNodeIndex = 0; - buildTree(0,numLeafNodes); + 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()) + if (m_useQuantization && !m_SubtreeHeaders.size()) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); @@ -200,37 +188,29 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized m_leafNodes.clear(); } - - - -void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface, const btVector3& aabbMin, const btVector3& aabbMax) { if (m_useQuantization) { + setQuantizationValues(aabbMin, aabbMax); - setQuantizationValues(aabbMin,aabbMax); - - updateBvhNodes(meshInterface,0,m_curNodeIndex,0); + updateBvhNodes(meshInterface, 0, m_curNodeIndex, 0); ///now update all subtree headers int i; - for (i=0;i<m_SubtreeHeaders.size();i++) + for (i = 0; i < m_SubtreeHeaders.size(); i++) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); } - - } else + } + else { - } } - - - -void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface, const btVector3& aabbMin, const btVector3& aabbMax) { //incrementally initialize quantization values btAssert(m_useQuantization); @@ -245,147 +225,140 @@ void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b ///we should update all quantization values, using updateBvhNodes(meshInterface); ///but we only update chunks that overlap the given aabb - - unsigned short quantizedQueryAabbMin[3]; - unsigned short quantizedQueryAabbMax[3]; - quantize(&quantizedQueryAabbMin[0],aabbMin,0); - quantize(&quantizedQueryAabbMax[0],aabbMax,1); + unsigned short quantizedQueryAabbMin[3]; + unsigned short quantizedQueryAabbMax[3]; + + quantize(&quantizedQueryAabbMin[0], aabbMin, 0); + quantize(&quantizedQueryAabbMax[0], aabbMax, 1); int i; - for (i=0;i<this->m_SubtreeHeaders.size();i++) + for (i = 0; i < this->m_SubtreeHeaders.size(); i++) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; //PCK: unsigned instead of bool - unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + 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); + updateBvhNodes(meshInterface, subtree.m_rootNodeIndex, subtree.m_rootNodeIndex + subtree.m_subtreeSize, i); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); } } - } -void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) +void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int firstNode, int endNode, int index) { (void)index; btAssert(m_useQuantization); - int curNodeSubPart=-1; + int curNodeSubPart = -1; //get access info to trianglemesh data - 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; - const btVector3& meshScaling = meshInterface->getScaling(); - - int i; - for (i=endNode-1;i>=firstNode;i--) + 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; + const btVector3& meshScaling = meshInterface->getScaling(); + + int i; + for (i = endNode - 1; i >= firstNode; i--) + { + btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; + if (curNode.isLeafNode()) { + //recalc aabb from triangle data + int 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; + } + //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, - btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; - if (curNode.isLeafNode()) + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + + for (int j = 2; j >= 0; j--) { - //recalc aabb from triangle data - int nodeSubPart = curNode.getPartId(); - int nodeTriangleIndex = curNode.getTriangleIndex(); - if (nodeSubPart != curNodeSubPart) + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } + if (type == PHY_FLOAT) { - 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); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3( + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } - //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, - - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - - - for (int j=2;j>=0;j--) + else { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); - } + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ())); } + } + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMin.setMin(triangleVerts[0]); + aabbMax.setMax(triangleVerts[0]); + aabbMin.setMin(triangleVerts[1]); + aabbMax.setMax(triangleVerts[1]); + aabbMin.setMin(triangleVerts[2]); + aabbMax.setMax(triangleVerts[2]); + + quantize(&curNode.m_quantizedAabbMin[0], aabbMin, 0); + quantize(&curNode.m_quantizedAabbMax[0], aabbMax, 1); + } + else + { + //combine aabb from both children - - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangleVerts[0]); - aabbMax.setMax(triangleVerts[0]); - aabbMin.setMin(triangleVerts[1]); - aabbMax.setMax(triangleVerts[1]); - aabbMin.setMin(triangleVerts[2]); - aabbMax.setMax(triangleVerts[2]); - - quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); - quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1); - - } else - { - //combine aabb from both children + btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i + 1]; - btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; - - btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : - &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; - + btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i + 2] : &m_quantizedContiguousNodes[i + 1 + leftChildNode->getEscapeIndex()]; + { + for (int i = 0; i < 3; i++) { - for (int i=0;i<3;i++) - { - curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; - if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i]) - curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i]; - - curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; - if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) - curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; - } + curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; + if (curNode.m_quantizedAabbMin[i] > rightChildNode->m_quantizedAabbMin[i]) + curNode.m_quantizedAabbMin[i] = rightChildNode->m_quantizedAabbMin[i]; + + curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; + if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) + curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; } } - } + } - if (curNodeSubPart >= 0) - meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); - - + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); } ///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) +btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) { - btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - + btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer, i_dataBufferSize, i_swapEndian); + //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 715961f5528..22f131c8b25 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -22,44 +22,35 @@ subject to the following restrictions: class btStridingMeshInterface; - ///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface. -ATTRIBUTE_ALIGNED16(class) btOptimizedBvh : public btQuantizedBvh +ATTRIBUTE_ALIGNED16(class) +btOptimizedBvh : public btQuantizedBvh { - public: BT_DECLARE_ALIGNED_ALLOCATOR(); protected: - public: - btOptimizedBvh(); virtual ~btOptimizedBvh(); - void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); + void build(btStridingMeshInterface * triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); - void refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax); + void refit(btStridingMeshInterface * triangles, const btVector3& aabbMin, const btVector3& aabbMax); - void refitPartial(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); + void updateBvhNodes(btStridingMeshInterface * meshInterface, int firstNode, int endNode, int index); /// Data buffer MUST be 16 byte aligned - virtual bool serializeInPlace(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const + virtual bool serializeInPlace(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const { - return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - + return btQuantizedBvh::serialize(o_alignedDataBuffer, i_dataBufferSize, i_swapEndian); } ///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); - - + static btOptimizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); }; - -#endif //BT_OPTIMIZED_BVH_H - - +#endif //BT_OPTIMIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index b831e20c2f9..e4bd7bb4d56 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -12,9 +12,9 @@ subject to the following restrictions: 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. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" #include "btConvexPolyhedron.h" @@ -23,11 +23,9 @@ subject to the following restrictions: #include "LinearMath/btGeometryUtil.h" #include "LinearMath/btGrahamScan2dConvexHull.h" - -btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), -m_polyhedron(0) +btPolyhedralConvexShape::btPolyhedralConvexShape() : btConvexInternalShape(), + m_polyhedron(0) { - } btPolyhedralConvexShape::~btPolyhedralConvexShape() @@ -39,79 +37,148 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape() } } - -bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) +void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron) { + if (m_polyhedron) + { + *m_polyhedron = polyhedron; + } + else + { + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16); + m_polyhedron = new (mem) btConvexPolyhedron(polyhedron); + } +} +bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) +{ if (m_polyhedron) { m_polyhedron->~btConvexPolyhedron(); btAlignedFree(m_polyhedron); } - - void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); + + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16); m_polyhedron = new (mem) btConvexPolyhedron; btAlignedObjectArray<btVector3> orgVertices; - for (int i=0;i<getNumVertices();i++) + for (int i = 0; i < getNumVertices(); i++) { btVector3& newVertex = orgVertices.expand(); - getVertex(i,newVertex); + getVertex(i, newVertex); } - + btConvexHullComputer conv; - + if (shiftVerticesByMargin) { btAlignedObjectArray<btVector3> planeEquations; - btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); + btGeometryUtil::getPlaneEquationsFromVertices(orgVertices, planeEquations); btAlignedObjectArray<btVector3> shiftedPlaneEquations; - for (int p=0;p<planeEquations.size();p++) + for (int p = 0; p < planeEquations.size(); p++) { - btVector3 plane = planeEquations[p]; - // btScalar margin = getMargin(); - plane[3] -= getMargin(); - shiftedPlaneEquations.push_back(plane); + btVector3 plane = planeEquations[p]; + // btScalar margin = getMargin(); + plane[3] -= getMargin(); + shiftedPlaneEquations.push_back(plane); } btAlignedObjectArray<btVector3> tmpVertices; - btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); - - conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); - } else + btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations, tmpVertices); + + conv.compute(&tmpVertices[0].getX(), sizeof(btVector3), tmpVertices.size(), 0.f, 0.f); + } + else { - - conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); + conv.compute(&orgVertices[0].getX(), sizeof(btVector3), orgVertices.size(), 0.f, 0.f); } +#ifndef BT_RECONSTRUCT_FACES + int numVertices = conv.vertices.size(); + m_polyhedron->m_vertices.resize(numVertices); + for (int p = 0; p < numVertices; p++) + { + m_polyhedron->m_vertices[p] = conv.vertices[p]; + } + + int v0, v1; + for (int j = 0; j < conv.faces.size(); j++) + { + btVector3 edges[3]; + int numEdges = 0; + btFace combinedFace; + const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]]; + v0 = edge->getSourceVertex(); + int prevVertex = v0; + combinedFace.m_indices.push_back(v0); + v1 = edge->getTargetVertex(); + while (v1 != v0) + { + btVector3 wa = conv.vertices[prevVertex]; + btVector3 wb = conv.vertices[v1]; + btVector3 newEdge = wb - wa; + newEdge.normalize(); + if (numEdges < 2) + edges[numEdges++] = newEdge; + + //face->addIndex(v1); + combinedFace.m_indices.push_back(v1); + edge = edge->getNextEdgeOfFace(); + prevVertex = v1; + int v01 = edge->getSourceVertex(); + v1 = edge->getTargetVertex(); + } + + btAssert(combinedFace.m_indices.size() > 2); + + btVector3 faceNormal = edges[0].cross(edges[1]); + faceNormal.normalize(); + + btScalar planeEq = 1e30f; + + for (int v = 0; v < combinedFace.m_indices.size(); v++) + { + btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal); + if (planeEq > eq) + { + planeEq = eq; + } + } + combinedFace.m_plane[0] = faceNormal.getX(); + combinedFace.m_plane[1] = faceNormal.getY(); + combinedFace.m_plane[2] = faceNormal.getZ(); + combinedFace.m_plane[3] = -planeEq; + + m_polyhedron->m_faces.push_back(combinedFace); + } + +#else //BT_RECONSTRUCT_FACES btAlignedObjectArray<btVector3> faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); btConvexHullComputer* convexUtil = &conv; - - btAlignedObjectArray<btFace> tmpFaces; + btAlignedObjectArray<btFace> tmpFaces; tmpFaces.resize(numFaces); int numVertices = convexUtil->vertices.size(); m_polyhedron->m_vertices.resize(numVertices); - for (int p=0;p<numVertices;p++) + for (int p = 0; p < numVertices; p++) { m_polyhedron->m_vertices[p] = convexUtil->vertices[p]; } - - for (int i=0;i<numFaces;i++) + for (int i = 0; i < numFaces; i++) { int face = convexUtil->faces[i]; //printf("face=%d\n",face); - const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; - const btConvexHullComputer::Edge* edge = firstEdge; + const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge; btVector3 edges[3]; int numEdges = 0; @@ -119,25 +186,23 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa do { - int src = edge->getSourceVertex(); tmpFaces[i].m_indices.push_back(src); int targ = edge->getTargetVertex(); btVector3 wa = convexUtil->vertices[src]; btVector3 wb = convexUtil->vertices[targ]; - btVector3 newEdge = wb-wa; + btVector3 newEdge = wb - wa; newEdge.normalize(); - if (numEdges<2) + if (numEdges < 2) edges[numEdges++] = newEdge; edge = edge->getNextEdgeOfFace(); - } while (edge!=firstEdge); + } while (edge != firstEdge); btScalar planeEq = 1e30f; - - if (numEdges==2) + if (numEdges == 2) { faceNormals[i] = edges[0].cross(edges[1]); faceNormals[i].normalize(); @@ -145,20 +210,19 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa tmpFaces[i].m_plane[1] = faceNormals[i].getY(); tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); tmpFaces[i].m_plane[3] = planeEq; - } else { - btAssert(0);//degenerate? + btAssert(0); //degenerate? faceNormals[i].setZero(); } - for (int v=0;v<tmpFaces[i].m_indices.size();v++) + for (int v = 0; v < tmpFaces[i].m_indices.size(); v++) { btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); - if (planeEq>eq) + if (planeEq > eq) { - planeEq=eq; + planeEq = eq; } } tmpFaces[i].m_plane[3] = -planeEq; @@ -166,89 +230,86 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa //merge coplanar faces and copy them to m_polyhedron - btScalar faceWeldThreshold= 0.999f; + btScalar faceWeldThreshold = 0.999f; btAlignedObjectArray<int> todoFaces; - for (int i=0;i<tmpFaces.size();i++) + for (int i = 0; i < tmpFaces.size(); i++) todoFaces.push_back(i); while (todoFaces.size()) { btAlignedObjectArray<int> coplanarFaceGroup; - int refFace = todoFaces[todoFaces.size()-1]; + int refFace = todoFaces[todoFaces.size() - 1]; coplanarFaceGroup.push_back(refFace); btFace& faceA = tmpFaces[refFace]; todoFaces.pop_back(); - btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); - for (int j=todoFaces.size()-1;j>=0;j--) + btVector3 faceNormalA(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]); + for (int j = todoFaces.size() - 1; j >= 0; j--) { int i = todoFaces[j]; btFace& faceB = tmpFaces[i]; - btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); - if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + btVector3 faceNormalB(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]); + if (faceNormalA.dot(faceNormalB) > faceWeldThreshold) { coplanarFaceGroup.push_back(i); todoFaces.remove(i); } } - bool did_merge = false; - if (coplanarFaceGroup.size()>1) + if (coplanarFaceGroup.size() > 1) { //do the merge: use Graham Scan 2d convex hull btAlignedObjectArray<GrahamVector3> orgpoints; - btVector3 averageFaceNormal(0,0,0); + btVector3 averageFaceNormal(0, 0, 0); - for (int i=0;i<coplanarFaceGroup.size();i++) + for (int i = 0; i < coplanarFaceGroup.size(); i++) { -// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + // m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); btFace& face = tmpFaces[coplanarFaceGroup[i]]; - btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); - averageFaceNormal+=faceNormal; - for (int f=0;f<face.m_indices.size();f++) + btVector3 faceNormal(face.m_plane[0], face.m_plane[1], face.m_plane[2]); + averageFaceNormal += faceNormal; + for (int f = 0; f < face.m_indices.size(); f++) { int orgIndex = face.m_indices[f]; btVector3 pt = m_polyhedron->m_vertices[orgIndex]; - + bool found = false; - for (int i=0;i<orgpoints.size();i++) + for (int i = 0; i < orgpoints.size(); i++) { //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) if (orgpoints[i].m_orgIndex == orgIndex) { - found=true; + found = true; break; } } if (!found) - orgpoints.push_back(GrahamVector3(pt,orgIndex)); + orgpoints.push_back(GrahamVector3(pt, orgIndex)); } } - - btFace combinedFace; - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; btAlignedObjectArray<GrahamVector3> hull; averageFaceNormal.normalize(); - GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal); + GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal); - for (int i=0;i<hull.size();i++) + for (int i = 0; i < hull.size(); i++) { combinedFace.m_indices.push_back(hull[i].m_orgIndex); - for(int k = 0; k < orgpoints.size(); k++) + for (int k = 0; k < orgpoints.size(); k++) { - if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) + if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex) { - orgpoints[k].m_orgIndex = -1; // invalidate... + orgpoints[k].m_orgIndex = -1; // invalidate... break; } } @@ -256,38 +317,41 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa // are there rejected vertices? bool reject_merge = false; - - - for(int i = 0; i < orgpoints.size(); i++) { - if(orgpoints[i].m_orgIndex == -1) - continue; // this is in the hull... + for (int i = 0; i < orgpoints.size(); i++) + { + if (orgpoints[i].m_orgIndex == -1) + continue; // this is in the hull... // this vertex is rejected -- is anybody else using this vertex? - for(int j = 0; j < tmpFaces.size(); j++) { - + for (int j = 0; j < tmpFaces.size(); j++) + { btFace& face = tmpFaces[j]; // is this a face of the current coplanar group? bool is_in_current_group = false; - for(int k = 0; k < coplanarFaceGroup.size(); k++) { - if(coplanarFaceGroup[k] == j) { + for (int k = 0; k < coplanarFaceGroup.size(); k++) + { + if (coplanarFaceGroup[k] == j) + { is_in_current_group = true; break; } } - if(is_in_current_group) // ignore this face... + if (is_in_current_group) // ignore this face... continue; // does this face use this rejected vertex? - for(int v = 0; v < face.m_indices.size(); v++) { - if(face.m_indices[v] == orgpoints[i].m_orgIndex) { + for (int v = 0; v < face.m_indices.size(); v++) + { + if (face.m_indices[v] == orgpoints[i].m_orgIndex) + { // this rejected vertex is used in another face -- reject merge reject_merge = true; break; } } - if(reject_merge) + if (reject_merge) break; } - if(reject_merge) + if (reject_merge) break; } @@ -298,34 +362,30 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa m_polyhedron->m_faces.push_back(combinedFace); } } - if(!did_merge) + if (!did_merge) { - for (int i=0;i<coplanarFaceGroup.size();i++) + for (int i = 0; i < coplanarFaceGroup.size(); i++) { btFace face = tmpFaces[coplanarFaceGroup[i]]; m_polyhedron->m_faces.push_back(face); } + } + } - } - - +#endif //BT_RECONSTRUCT_FACES - } - m_polyhedron->initialize(); return true; } #ifndef MIN - #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) +#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) #endif -btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - - - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); #ifndef __SPU__ int i; btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -334,37 +394,36 @@ btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } btVector3 vtx; btScalar newDot; - for( int k = 0; k < getNumVertices(); k += 128 ) - { - btVector3 temp[128]; - int inner_count = MIN(getNumVertices() - k, 128); - for( i = 0; i < inner_count; i++ ) - getVertex(i,temp[i]); - i = (int) vec.maxDot( temp, inner_count, newDot); + for (int k = 0; k < getNumVertices(); k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(getNumVertices() - k, 128); + for (i = 0; i < inner_count; i++) + getVertex(i, temp[i]); + i = (int)vec.maxDot(temp, inner_count, newDot); if (newDot > maxDot) { maxDot = newDot; supVec = temp[i]; - } - } - -#endif //__SPU__ + } + } + +#endif //__SPU__ return supVec; } - - -void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { #ifndef __SPU__ int i; @@ -372,36 +431,34 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( btVector3 vtx; btScalar newDot; - for (i=0;i<numVectors;i++) + for (i = 0; i < numVectors; i++) { supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { - const btVector3& vec = vectors[j]; - - for( int k = 0; k < getNumVertices(); k += 128 ) - { - btVector3 temp[128]; - int inner_count = MIN(getNumVertices() - k, 128); - for( i = 0; i < inner_count; i++ ) - getVertex(i,temp[i]); - i = (int) vec.maxDot( temp, inner_count, newDot); - if (newDot > supportVerticesOut[j][3]) - { + const btVector3& vec = vectors[j]; + + for (int k = 0; k < getNumVertices(); k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(getNumVertices() - k, 128); + for (i = 0; i < inner_count; i++) + getVertex(i, temp[i]); + i = (int)vec.maxDot(temp, inner_count, newDot); + if (newDot > supportVerticesOut[j][3]) + { supportVerticesOut[j] = temp[i]; supportVerticesOut[j][3] = newDot; - } - } - } + } + } + } -#endif //__SPU__ +#endif //__SPU__ } - - -void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { #ifndef __SPU__ //not yet, return box inertia @@ -410,81 +467,77 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine btTransform ident; ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); // This already contains the margin - 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()); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btVector3 aabbMin, aabbMax; + getAabb(ident, aabbMin, aabbMax); // This already contains the margin + 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()); + 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 //__SPU__ + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); +#endif //__SPU__ } - - -void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) +void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) { btConvexInternalShape::setLocalScaling(scaling); recalcLocalAabb(); } btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape() -:btPolyhedralConvexShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) + : btPolyhedralConvexShape(), + m_localAabbMin(1, 1, 1), + m_localAabbMax(-1, -1, -1), + m_isLocalAabbValid(false) { } -void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); } -void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() +void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() { m_isLocalAabbValid = true; - - #if 1 + +#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(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.) - }; - + { + 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 ) + + for (int i = 0; i < 3; ++i) { m_localAabbMax[i] = _supporting[i][i]; m_localAabbMin[i] = _supporting[i + 3][i]; } - #else +#else - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); m_localAabbMax[i] = tmp[i]; @@ -492,9 +545,5 @@ void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() tmp = localGetSupportingVertex(vec); m_localAabbMin[i] = tmp[i]; } - #endif +#endif } - - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 961d001a9dd..b3ffab7a23c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -20,20 +20,15 @@ subject to the following restrictions: #include "btConvexInternalShape.h" class btConvexPolyhedron; - ///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes. -ATTRIBUTE_ALIGNED16(class) btPolyhedralConvexShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btPolyhedralConvexShape : public btConvexInternalShape { - - protected: - btConvexPolyhedron* m_polyhedron; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btPolyhedralConvexShape(); @@ -41,76 +36,71 @@ public: ///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) ///experimental/work-in-progress - virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0); + virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin = 0); - const btConvexPolyhedron* getConvexPolyhedron() const + virtual void setPolyhedralFeatures(btConvexPolyhedron & polyhedron); + + const btConvexPolyhedron* getConvexPolyhedron() const { return m_polyhedron; } //brute force implementations - 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) const; - - - virtual int getNumVertices() const = 0 ; + 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) const; + + virtual int getNumVertices() const = 0; virtual int getNumEdges() const = 0; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0; - virtual void getVertex(int i,btVector3& vtx) const = 0; - virtual int getNumPlanes() const = 0; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0; -// virtual int getIndex(int i) const = 0 ; - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0; - -}; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const = 0; + virtual void getVertex(int i, btVector3& vtx) const = 0; + virtual int getNumPlanes() const = 0; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const = 0; + // virtual int getIndex(int i) const = 0 ; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const = 0; +}; ///The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape { + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - bool m_isLocalAabbValid; - protected: - - void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) + void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax) { m_isLocalAabbValid = true; m_localAabbMin = aabbMin; m_localAabbMax = aabbMax; } - inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const + inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const { btAssert(m_isLocalAabbValid); aabbMin = m_localAabbMin; aabbMax = m_localAabbMax; } -public: - +protected: btPolyhedralConvexAabbCachingShape(); - - inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const - { +public: + inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const + { //lazy evaluation of local aabb btAssert(m_isLocalAabbValid); - btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); + btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax); } - virtual void setLocalScaling(const btVector3& scaling); - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); - void recalcLocalAabb(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + void recalcLocalAabb(); }; -#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H +#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp index 6a337c786c4..f4273199749 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -13,11 +13,10 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btScaledBvhTriangleMeshShape.h" -btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) -:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape, const btVector3& localScaling) + : m_localScaling(localScaling), m_bvhTriMeshShape(childShape) { m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; } @@ -26,55 +25,51 @@ btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() { } - class btScaledTriangleCallback : public btTriangleCallback { btTriangleCallback* m_originalCallback; - btVector3 m_localScaling; + btVector3 m_localScaling; public: - - btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) - :m_originalCallback(originalCallback), - m_localScaling(localScaling) + btScaledTriangleCallback(btTriangleCallback* originalCallback, const 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); + 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 +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; + 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]; scaledAabbMin[3] = 0.f; - + 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]; scaledAabbMax[3] = 0.f; - - - m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); -} + m_bvhTriMeshShape->processAllTriangles(&scaledCallback, scaledAabbMin, scaledAabbMax); +} -void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); @@ -89,22 +84,21 @@ void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& a 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); + 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(); + localHalfExtents += btVector3(margin, margin, margin); + btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); aabbMin = center - extent; aabbMax = center + extent; - } -void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } @@ -114,8 +108,8 @@ const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const return m_localScaling; } -void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { ///don't make this a movable object! -// btAssert(0); + // btAssert(0); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h index 39049eaf083..4d6feb61af5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -18,78 +18,69 @@ subject to the following restrictions: #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 +ATTRIBUTE_ALIGNED16(class) +btScaledBvhTriangleMeshShape : public btConcaveShape { - - - btVector3 m_localScaling; + btVector3 m_localScaling; - btBvhTriangleMeshShape* m_bvhTriMeshShape; + btBvhTriangleMeshShape* m_bvhTriMeshShape; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling); + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape * childShape, const btVector3& localScaling); virtual ~btScaledBvhTriangleMeshShape(); - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling); + 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 calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - btBvhTriangleMeshShape* getChildShape() + btBvhTriangleMeshShape* getChildShape() { return m_bvhTriMeshShape; } - const btBvhTriangleMeshShape* getChildShape() const + const btBvhTriangleMeshShape* getChildShape() const { return m_bvhTriMeshShape; } //debugging - virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + virtual const char* getName() const { return "SCALEDBVHTRIANGLEMESH"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btScaledTriangleMeshShapeData +struct btScaledTriangleMeshShapeData { - btTriangleMeshShapeData m_trimeshShapeData; + btTriangleMeshShapeData m_trimeshShapeData; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; }; - -SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const { return sizeof(btScaledTriangleMeshShapeData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*) dataBuffer; - m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData,serializer); + btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*)dataBuffer; + m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData, serializer); scaledMeshData->m_trimeshShapeData.m_collisionShapeData.m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; m_localScaling.serializeFloat(scaledMeshData->m_localScaling); return "btScaledTriangleMeshShapeData"; } - -#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp new file mode 100644 index 00000000000..4a95dbea4f2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -0,0 +1,92 @@ +#include "btSdfCollisionShape.h" +#include "btMiniSDF.h" +#include "LinearMath/btAabbUtil2.h" + +struct btSdfCollisionShapeInternalData +{ + btVector3 m_localScaling; + btScalar m_margin; + btMiniSDF m_sdf; + + btSdfCollisionShapeInternalData() + : m_localScaling(1, 1, 1), + m_margin(0) + { + } +}; + +bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes) +{ + bool valid = m_data->m_sdf.load(sdfData, sizeInBytes); + return valid; +} +btSdfCollisionShape::btSdfCollisionShape() +{ + m_shapeType = SDF_SHAPE_PROXYTYPE; + m_data = new btSdfCollisionShapeInternalData(); + + //"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf"); + /*unsigned int field_id=0; + Eigen::Vector3d x (1,10,1); + Eigen::Vector3d gradient; + double dist = m_data->m_sdf.interpolate(field_id, x, &gradient); + printf("dist=%g\n", dist); + */ +} +btSdfCollisionShape::~btSdfCollisionShape() +{ + delete m_data; +} + +void btSdfCollisionShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const +{ + btAssert(m_data->m_sdf.isValid()); + btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min; + btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max; + btScalar margin(0); + btTransformAabb(localAabbMin, localAabbMax, margin, t, aabbMin, aabbMax); +} + +void btSdfCollisionShape::setLocalScaling(const btVector3& scaling) +{ + m_data->m_localScaling = scaling; +} +const btVector3& btSdfCollisionShape::getLocalScaling() const +{ + return m_data->m_localScaling; +} +void btSdfCollisionShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const +{ + inertia.setValue(0, 0, 0); +} +const char* btSdfCollisionShape::getName() const +{ + return "btSdfCollisionShape"; +} +void btSdfCollisionShape::setMargin(btScalar margin) +{ + m_data->m_margin = margin; +} +btScalar btSdfCollisionShape::getMargin() const +{ + return m_data->m_margin; +} + +void btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const +{ + //not yet +} + +bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal) +{ + int field = 0; + btVector3 grad; + double dist; + bool hasResult = m_data->m_sdf.interpolate(field, dist, ptInSDF, &grad); + if (hasResult) + { + normal.setValue(grad[0], grad[1], grad[2]); + distOut = dist; + } + return hasResult; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h new file mode 100644 index 00000000000..3989d6245ed --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h @@ -0,0 +1,29 @@ +#ifndef BT_SDF_COLLISION_SHAPE_H +#define BT_SDF_COLLISION_SHAPE_H + +#include "btConcaveShape.h" + +class btSdfCollisionShape : public btConcaveShape +{ + struct btSdfCollisionShapeInternalData* m_data; + +public: + btSdfCollisionShape(); + virtual ~btSdfCollisionShape(); + + bool initializeSDF(const char* sdfData, int sizeInBytes); + + 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 const char* getName() const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const; + + bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal); +}; + +#endif //BT_SDF_COLLISION_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp index 3beaf865801..a2c490faf9d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -15,51 +15,48 @@ subject to the following restrictions: //btShapeHull was implemented by John McCutchan. - #include "btShapeHull.h" #include "LinearMath/btConvexHull.h" #define NUM_UNITSPHERE_POINTS 42 +#define NUM_UNITSPHERE_POINTS_HIGHRES 256 -btShapeHull::btShapeHull (const btConvexShape* shape) +btShapeHull::btShapeHull(const btConvexShape* shape) { m_shape = shape; - m_vertices.clear (); + m_vertices.clear(); m_indices.clear(); m_numIndices = 0; } -btShapeHull::~btShapeHull () +btShapeHull::~btShapeHull() { - m_indices.clear(); - m_vertices.clear (); + m_indices.clear(); + m_vertices.clear(); } -bool -btShapeHull::buildHull (btScalar /*margin*/) +bool btShapeHull::buildHull(btScalar /*margin*/, int highres) { - 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); - getUnitSpherePoints()[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - - btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + + int numSampleDirections = highres ? NUM_UNITSPHERE_POINTS_HIGHRES : NUM_UNITSPHERE_POINTS; + btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; for (i = 0; i < numSampleDirections; i++) { - supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]); + supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); } + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int s = 0; s < numPDA; s++) + { + btVector3 norm; + m_shape->getPreferredPenetrationDirection(s, norm); + supportPoints[i++] = m_shape->localGetSupportingVertex(norm); + numSampleDirections++; + } + } HullDesc hd; hd.mFlags = QF_TRIANGLES; hd.mVcount = static_cast<unsigned int>(numSampleDirections); @@ -69,18 +66,17 @@ btShapeHull::buildHull (btScalar /*margin*/) hd.mVertexStride = sizeof(btVector3); #else hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof (btVector3); + hd.mVertexStride = sizeof(btVector3); #endif HullLibrary hl; HullResult hr; - if (hl.CreateConvexHull (hd, hr) == QE_FAIL) + if (hl.CreateConvexHull(hd, hr) == QE_FAIL) { return false; } - m_vertices.resize (static_cast<int>(hr.mNumOutputVertices)); - + m_vertices.resize(static_cast<int>(hr.mNumOutputVertices)); for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++) { @@ -94,77 +90,332 @@ btShapeHull::buildHull (btScalar /*margin*/) } // free temporary hull result that we just copied - hl.ReleaseResult (hr); + hl.ReleaseResult(hr); return true; } -int -btShapeHull::numTriangles () const +int btShapeHull::numTriangles() const { return static_cast<int>(m_numIndices / 3); } -int -btShapeHull::numVertices () const +int btShapeHull::numVertices() const { - return m_vertices.size (); + return m_vertices.size(); } -int -btShapeHull::numIndices () const +int btShapeHull::numIndices() const { return static_cast<int>(m_numIndices); } - -btVector3* btShapeHull::getUnitSpherePoints() +btVector3* btShapeHull::getUnitSpherePoints(int highres) { - static btVector3 sUnitSpherePoints[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)) - }; + static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)), + btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)), + btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)), + btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)), + btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)), + btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)), + btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)), + btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)), + btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)), + btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)), + btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)), + btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)), + btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)), + btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)), + btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)), + btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)), + btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)), + btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)), + btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)), + btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)), + btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)), + btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)), + btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)), + btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)), + btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)), + btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)), + btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)), + btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)), + btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)), + btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)), + btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)), + btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)), + btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)), + btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)), + btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)), + btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)), + btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)), + btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)), + btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)), + btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)), + btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)), + btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)), + btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)), + btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)), + btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)), + btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)), + btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)), + btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)), + btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)), + btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)), + btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)), + btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)), + btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)), + btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)), + btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)), + btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)), + btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)), + btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)), + btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)), + btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)), + btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)), + btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)), + btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)), + btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)), + btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)), + btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)), + btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)), + btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)), + btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)), + btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)), + btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)), + btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)), + btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)), + btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)), + btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)), + btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)), + btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)), + btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)), + btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)), + btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)), + btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)), + btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)), + btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)), + btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)), + btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)), + btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)), + btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)), + btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)), + btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)), + btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)), + btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)), + btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)), + btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)), + btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)), + btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)), + btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)), + btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)), + btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)), + btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)), + btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)), + btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)), + btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)), + btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)), + btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)), + btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)), + btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)), + btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)), + btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)), + btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)), + btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)), + btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)), + btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)), + btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)), + btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)), + btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)), + btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)), + btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)), + btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)), + btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)), + btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)), + btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)), + btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)), + btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)), + btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)), + btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)), + btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)), + btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)), + btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)), + btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)), + btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)), + btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)), + btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)), + btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)), + btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)), + btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)), + btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)), + btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)), + btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)), + btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)), + btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)), + btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)), + btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)), + btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)), + btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)), + btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)), + btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)), + btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)), + btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)), + btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)), + btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)), + btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)), + btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)), + btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)), + btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)), + btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)), + btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)), + btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)), + btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)), + btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)), + btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)), + btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)), + btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)), + btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)), + btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)), + btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)), + btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)), + btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)), + btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)), + btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)), + btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)), + btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)), + btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)), + btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)), + btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)), + btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)), + btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)), + btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)), + btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)), + btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)), + btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)), + btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)), + btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)), + btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)), + btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)), + btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)), + btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)), + btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)), + btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)), + btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)), + btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)), + btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)), + btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)), + btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)), + btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)), + btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)), + btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)), + btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)), + btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)), + btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)), + btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)), + btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)), + btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)), + btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)), + btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)), + btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)), + btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)), + btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)), + btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)), + btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)), + btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)), + btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)), + btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)), + btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)), + btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)), + btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)), + btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)), + btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)), + btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)), + btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)), + btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)), + btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)), + btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)), + btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)), + btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)), + btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)), + btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)), + btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)), + btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)), + btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)), + btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)), + btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)), + btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)), + btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)), + btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)), + btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)), + btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)), + btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)), + btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)), + btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)), + btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)), + btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)), + btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)), + btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)), + btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)), + btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)), + btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)), + btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)), + btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)), + btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)), + btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)), + btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)), + btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)), + btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)), + btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)), + btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)), + btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)), + }; + static btVector3 sUnitSpherePoints[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))}; + if (highres) + return sUnitSpherePointsHighres; return sUnitSpherePoints; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h index e959f198b69..54439f9ca25 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -21,32 +21,31 @@ subject to the following restrictions: #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. -ATTRIBUTE_ALIGNED16(class) btShapeHull +ATTRIBUTE_ALIGNED16(class) +btShapeHull { protected: - btAlignedObjectArray<btVector3> m_vertices; btAlignedObjectArray<unsigned int> m_indices; unsigned int m_numIndices; const btConvexShape* m_shape; - static btVector3* getUnitSpherePoints(); + static btVector3* getUnitSpherePoints(int highres = 0); public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btShapeHull (const btConvexShape* shape); - ~btShapeHull (); - bool buildHull (btScalar margin); + btShapeHull(const btConvexShape* shape); + ~btShapeHull(); + + bool buildHull(btScalar margin, int highres = 0); - int numTriangles () const; - int numVertices () const; - int numIndices () const; + int numTriangles() const; + int numVertices() const; + int numIndices() const; const btVector3* getVertexPointer() const { @@ -58,4 +57,4 @@ public: } }; -#endif //BT_SHAPE_HULL_H +#endif //BT_SHAPE_HULL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp index b9a736c0fdd..027db2e1046 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -18,54 +18,48 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" -btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { (void)vec; - return btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); + return btVector3(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { (void)vectors; - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i].setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + supportVerticesOut[i].setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } - -btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex; supVertex = localGetSupportingVertexWithoutMargin(vec); btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; return supVertex; } - //broken due to scaling -void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btSphereShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { const btVector3& center = t.getOrigin(); - btVector3 extent(getMargin(),getMargin(),getMargin()); + btVector3 extent(getMargin(), getMargin(), getMargin()); aabbMin = center - extent; aabbMax = center + extent; } - - -void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin(); - inertia.setValue(elem,elem,elem); - + btScalar elem = btScalar(0.4) * mass * getMargin() * getMargin(); + inertia.setValue(elem, elem, elem); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h index b192efeeb8d..75e4fd8e189 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -16,58 +16,56 @@ subject to the following restrictions: #define BT_SPHERE_MINKOWSKI_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btSphereShape implements an implicit sphere, centered around a local origin with radius. -ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btSphereShape : public btConvexInternalShape { - public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btSphereShape (btScalar radius) : btConvexInternalShape () + btSphereShape(btScalar radius) : btConvexInternalShape() { m_shapeType = SPHERE_SHAPE_PROXYTYPE; + m_localScaling.setValue(1.0, 1.0, 1.0); + m_implicitShapeDimensions.setZero(); m_implicitShapeDimensions.setX(radius); m_collisionMargin = radius; + m_padding = 0; } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + //notice that the vectors should be unit length + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();} + btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX(); } - void setUnscaledRadius(btScalar radius) + void setUnscaledRadius(btScalar radius) { m_implicitShapeDimensions.setX(radius); btConvexInternalShape::setMargin(radius); } //debugging - virtual const char* getName()const {return "SPHERE";} + virtual const char* getName() const { return "SPHERE"; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { btConvexInternalShape::setMargin(margin); } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { //to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case //this means, non-uniform scaling is not supported anymore return getRadius(); } - - }; - -#endif //BT_SPHERE_MINKOWSKI_H +#endif //BT_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index d17141e3f20..9238c919d51 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -17,24 +17,18 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" - -btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant) -: btConcaveShape (), m_planeNormal(planeNormal.normalized()), -m_planeConstant(planeConstant), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) +btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal, btScalar planeConstant) + : btConcaveShape(), m_planeNormal(planeNormal.normalized()), m_planeConstant(planeConstant), m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)) { m_shapeType = STATIC_PLANE_PROXYTYPE; // btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) ); } - btStaticPlaneShape::~btStaticPlaneShape() { } - - -void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btStaticPlaneShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { (void)t; /* @@ -47,57 +41,49 @@ void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVecto aabbMax.setMax(center - infvec*m_planeNormal); */ - aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); } - - - -void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); btScalar radius = halfExtents.length(); btVector3 center = (aabbMax + aabbMin) * btScalar(0.5); - + //this is where the triangles are generated, given AABB and plane equation (normal/constant) - btVector3 tangentDir0,tangentDir1; + btVector3 tangentDir0, tangentDir1; //tangentDir0/tangentDir1 can be precalculated - btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1); + btPlaneSpace1(m_planeNormal, tangentDir0, tangentDir1); - btVector3 supVertex0,supVertex1; + btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant) * m_planeNormal; - btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; - btVector3 triangle[3]; - triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius; - triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius; - triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius; - - callback->processTriangle(triangle,0,0); + triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; + triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; + triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; - triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius; - triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius; - triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius; + callback->processTriangle(triangle, 0, 0); - callback->processTriangle(triangle,0,1); + triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; + triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; + triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; + callback->processTriangle(triangle, 0, 1); } -void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btStaticPlaneShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) +void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index e6e32883959..1cda8bbc75b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -18,88 +18,86 @@ subject to the following restrictions: #include "btConcaveShape.h" - ///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane. -ATTRIBUTE_ALIGNED16(class) btStaticPlaneShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btStaticPlaneShape : public btConcaveShape { protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - - btVector3 m_planeNormal; - btScalar m_planeConstant; - btVector3 m_localScaling; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + + btVector3 m_planeNormal; + btScalar m_planeConstant; + btVector3 m_localScaling; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + btStaticPlaneShape(const btVector3& planeNormal, btScalar planeConstant); virtual ~btStaticPlaneShape(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - 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 processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; - - const btVector3& getPlaneNormal() const + + const btVector3& getPlaneNormal() const { - return m_planeNormal; + return m_planeNormal; } - const btScalar& getPlaneConstant() const + const btScalar& getPlaneConstant() const { - return m_planeConstant; + return m_planeConstant; } //debugging - virtual const char* getName()const {return "STATICPLANE";} + virtual const char* getName() const { return "STATICPLANE"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btStaticPlaneShapeData +struct btStaticPlaneShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; - btVector3FloatData m_localScaling; - btVector3FloatData m_planeNormal; - float m_planeConstant; - char m_pad[4]; + btVector3FloatData m_localScaling; + btVector3FloatData m_planeNormal; + float m_planeConstant; + char m_pad[4]; }; - -SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const { return sizeof(btStaticPlaneShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*) dataBuffer; - btCollisionShape::serialize(&planeData->m_collisionShapeData,serializer); + btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)dataBuffer; + btCollisionShape::serialize(&planeData->m_collisionShapeData, serializer); m_localScaling.serializeFloat(planeData->m_localScaling); m_planeNormal.serializeFloat(planeData->m_planeNormal); planeData->m_planeConstant = float(m_planeConstant); - - return "btStaticPlaneShapeData"; -} - - -#endif //BT_STATIC_PLANE_SHAPE_H + // Fill padding with zeros to appease msan. + planeData->m_pad[0] = 0; + planeData->m_pad[1] = 0; + planeData->m_pad[2] = 0; + planeData->m_pad[3] = 0; + return "btStaticPlaneShapeData"; +} +#endif //BT_STATIC_PLANE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index b3d449676b9..eb288e99c91 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -18,32 +18,30 @@ subject to the following restrictions: btStridingMeshInterface::~btStridingMeshInterface() { - } - -void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { (void)aabbMin; (void)aabbMax; int numtotalphysicsverts = 0; - int part,graphicssubparts = getNumSubParts(); - const unsigned char * vertexbase; - const unsigned char * indexbase; + int part, graphicssubparts = getNumSubParts(); + const unsigned char* vertexbase; + const unsigned char* indexbase; int indexstride; PHY_ScalarType type; PHY_ScalarType gfxindextype; - int stride,numverts,numtriangles; + int stride, numverts, numtriangles; int gfxindex; btVector3 triangle[3]; btVector3 meshScaling = getScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype - for (part=0;part<graphicssubparts ;part++) + for (part = 0; part < graphicssubparts; part++) { - getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part); - numtotalphysicsverts+=numtriangles*3; //upper bound + getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); + numtotalphysicsverts += numtriangles * 3; //upper bound ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build ///so disable this feature by default @@ -51,143 +49,141 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde switch (type) { - case PHY_FLOAT: - { - - float* graphicsbase; - - switch (gfxindextype) - { - case PHY_INTEGER: - { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) - { - unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - case PHY_SHORT: - { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) - { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - case PHY_UCHAR: - { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) - { - unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - default: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); - } - break; - } - - case PHY_DOUBLE: + case PHY_FLOAT: + { + float* graphicsbase; + + switch (gfxindextype) + { + case PHY_INTEGER: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + } + break; + } + + case PHY_DOUBLE: { double* graphicsbase; switch (gfxindextype) { - case PHY_INTEGER: + case PHY_INTEGER: { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); - graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - case PHY_SHORT: + case PHY_SHORT: { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); - graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - case PHY_UCHAR: + case PHY_UCHAR: { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); - graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - default: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); } break; } - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + default: + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } } -void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax) +void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin, btVector3& aabbMax) { - - struct AabbCalculationCallback : public btInternalTriangleIndexCallback + struct AabbCalculationCallback : public btInternalTriangleIndexCallback { - btVector3 m_aabbMin; - btVector3 m_aabbMax; + btVector3 m_aabbMin; + btVector3 m_aabbMax; AabbCalculationCallback() { - m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { (void)partId; (void)triangleIndex; @@ -202,21 +198,19 @@ void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVecto }; //first calculate the total aabb for all triangles - AabbCalculationCallback aabbCallback; - aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax); + AabbCalculationCallback aabbCallback; + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + InternalProcessAllTriangles(&aabbCallback, aabbMin, aabbMax); aabbMin = aabbCallback.m_aabbMin; aabbMax = aabbCallback.m_aabbMax; } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const { - btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer; + btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*)dataBuffer; trimeshData->m_numMeshParts = getNumSubParts(); @@ -226,29 +220,28 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s if (trimeshData->m_numMeshParts) { - btChunk* chunk = serializer->allocate(sizeof(btMeshPartData),trimeshData->m_numMeshParts); + btChunk* chunk = serializer->allocate(sizeof(btMeshPartData), trimeshData->m_numMeshParts); btMeshPartData* memPtr = (btMeshPartData*)chunk->m_oldPtr; - trimeshData->m_meshPartsPtr = (btMeshPartData *)serializer->getUniquePointer(memPtr); - + trimeshData->m_meshPartsPtr = (btMeshPartData*)serializer->getUniquePointer(memPtr); - // int numtotalphysicsverts = 0; - int part,graphicssubparts = getNumSubParts(); - const unsigned char * vertexbase; - const unsigned char * indexbase; + // int numtotalphysicsverts = 0; + int part, graphicssubparts = getNumSubParts(); + const unsigned char* vertexbase; + const unsigned char* indexbase; int indexstride; PHY_ScalarType type; PHY_ScalarType gfxindextype; - int stride,numverts,numtriangles; + int stride, numverts, numtriangles; int gfxindex; - // btVector3 triangle[3]; + // btVector3 triangle[3]; - // btVector3 meshScaling = getScaling(); + // btVector3 meshScaling = getScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype - for (part=0;part<graphicssubparts ;part++,memPtr++) + for (part = 0; part < graphicssubparts; part++, memPtr++) { - getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part); - memPtr->m_numTriangles = numtriangles;//indices = 3*numtriangles + getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); + memPtr->m_numTriangles = numtriangles; //indices = 3*numtriangles memPtr->m_numVertices = numverts; memPtr->m_indices16 = 0; memPtr->m_indices32 = 0; @@ -257,44 +250,46 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s memPtr->m_vertices3f = 0; memPtr->m_vertices3d = 0; - switch (gfxindextype) { - case PHY_INTEGER: + case PHY_INTEGER: { - int numindices = numtriangles*3; - + int numindices = numtriangles * 3; + if (numindices) { - btChunk* chunk = serializer->allocate(sizeof(btIntIndexData),numindices); + btChunk* chunk = serializer->allocate(sizeof(btIntIndexData), numindices); btIntIndexData* tmpIndices = (btIntIndexData*)chunk->m_oldPtr; memPtr->m_indices32 = (btIntIndexData*)serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); - tmpIndices[gfxindex*3].m_value = tri_indices[0]; - tmpIndices[gfxindex*3+1].m_value = tri_indices[1]; - tmpIndices[gfxindex*3+2].m_value = tri_indices[2]; + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + tmpIndices[gfxindex * 3].m_value = tri_indices[0]; + tmpIndices[gfxindex * 3 + 1].m_value = tri_indices[1]; + tmpIndices[gfxindex * 3 + 2].m_value = tri_indices[2]; } - serializer->finalizeChunk(chunk,"btIntIndexData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btIntIndexData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - case PHY_SHORT: + case PHY_SHORT: { if (numtriangles) { - btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData),numtriangles); + btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData), numtriangles); btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + memPtr->m_3indices16 = (btShortIntIndexTripletData*)serializer->getUniquePointer(tmpIndices); + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + // Fill padding with zeros to appease msan. + tmpIndices[gfxindex].m_pad[0] = 0; + tmpIndices[gfxindex].m_pad[1] = 0; } - serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btShortIntIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } @@ -302,21 +297,23 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s { if (numtriangles) { - btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData),numtriangles); + btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData), numtriangles); btCharIndexTripletData* tmpIndices = (btCharIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices8 = (btCharIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + memPtr->m_3indices8 = (btCharIndexTripletData*)serializer->getUniquePointer(tmpIndices); + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + // Fill padding with zeros to appease msan. + tmpIndices[gfxindex].m_pad = 0; } - serializer->finalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btCharIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - default: + default: { btAssert(0); //unknown index type @@ -325,56 +322,58 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s switch (type) { - case PHY_FLOAT: - { - float* graphicsbase; - - if (numverts) - { - btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData),numverts); - btVector3FloatData* tmpVertices = (btVector3FloatData*) chunk->m_oldPtr; - memPtr->m_vertices3f = (btVector3FloatData *)serializer->getUniquePointer(tmpVertices); - for (int i=0;i<numverts;i++) - { - graphicsbase = (float*)(vertexbase+i*stride); - tmpVertices[i].m_floats[0] = graphicsbase[0]; - tmpVertices[i].m_floats[1] = graphicsbase[1]; - tmpVertices[i].m_floats[2] = graphicsbase[2]; - } - serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); - } - break; + case PHY_FLOAT: + { + float* graphicsbase; + + if (numverts) + { + btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData), numverts); + btVector3FloatData* tmpVertices = (btVector3FloatData*)chunk->m_oldPtr; + memPtr->m_vertices3f = (btVector3FloatData*)serializer->getUniquePointer(tmpVertices); + for (int i = 0; i < numverts; i++) + { + graphicsbase = (float*)(vertexbase + i * stride); + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); + } + break; } - case PHY_DOUBLE: + case PHY_DOUBLE: { if (numverts) { - btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData),numverts); - btVector3DoubleData* tmpVertices = (btVector3DoubleData*) chunk->m_oldPtr; - memPtr->m_vertices3d = (btVector3DoubleData *) serializer->getUniquePointer(tmpVertices); - for (int i=0;i<numverts;i++) - { - double* graphicsbase = (double*)(vertexbase+i*stride);//for now convert to float, might leave it at double - tmpVertices[i].m_floats[0] = graphicsbase[0]; - tmpVertices[i].m_floats[1] = graphicsbase[1]; - tmpVertices[i].m_floats[2] = graphicsbase[2]; - } - serializer->finalizeChunk(chunk,"btVector3DoubleData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData), numverts); + btVector3DoubleData* tmpVertices = (btVector3DoubleData*)chunk->m_oldPtr; + memPtr->m_vertices3d = (btVector3DoubleData*)serializer->getUniquePointer(tmpVertices); + for (int i = 0; i < numverts; i++) + { + double* graphicsbase = (double*)(vertexbase + i * stride); //for now convert to float, might leave it at double + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk, "btVector3DoubleData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + default: + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } - serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btMeshPartData", BT_ARRAY_CODE, chunk->m_oldPtr); } + // Fill padding with zeros to appease msan. + memset(trimeshData->m_padding, 0, sizeof(trimeshData->m_padding)); m_scaling.serializeFloat(trimeshData->m_scaling); return "btStridingMeshInterfaceData"; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index 9fbe139768d..68a41dfb450 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -20,110 +20,102 @@ subject to the following restrictions: #include "btTriangleCallback.h" #include "btConcaveShape.h" - - - - /// 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. -ATTRIBUTE_ALIGNED16(class ) btStridingMeshInterface +ATTRIBUTE_ALIGNED16(class) +btStridingMeshInterface { - protected: - - btVector3 m_scaling; - - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.)) - { - - } - - virtual ~btStridingMeshInterface(); - - - - virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - ///brute force method to calculate aabb - void calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax); - - /// get read and write access to a subpart of a triangle mesh - /// this subpart has a continuous array of vertices and indices - /// in this way the mesh can be handled as chunks of memory with striding - /// very similar to OpenGL vertexarray support - /// make a call to unLockVertexBase when the read and write access is finished - 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)=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=0; - - /// 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)=0; - - virtual void unLockReadOnlyVertexBase(int subpart) const=0; - - - /// getNumSubParts returns the number of seperate subparts - /// each subpart has a continuous array of vertices and indices - virtual int getNumSubParts() const=0; - - 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 - { - (void) aabbMin; - (void) aabbMax; - } - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const - { - (void) aabbMin; - (void) aabbMax; - } - - const btVector3& getScaling() const { - return m_scaling; - } - void setScaling(const btVector3& scaling) - { - m_scaling = scaling; - } - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - +protected: + btVector3 m_scaling; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btStridingMeshInterface() : m_scaling(btScalar(1.), btScalar(1.), btScalar(1.)) + { + } + + virtual ~btStridingMeshInterface(); + + virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; + + ///brute force method to calculate aabb + void calculateAabbBruteForce(btVector3 & aabbMin, btVector3 & aabbMax); + + /// get read and write access to a subpart of a triangle mesh + /// this subpart has a continuous array of vertices and indices + /// in this way the mesh can be handled as chunks of memory with striding + /// very similar to OpenGL vertexarray support + /// make a call to unLockVertexBase when the read and write access is finished + 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) = 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 = 0; + + /// 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) = 0; + + virtual void unLockReadOnlyVertexBase(int subpart) const = 0; + + /// getNumSubParts returns the number of separate subparts + /// each subpart has a continuous array of vertices and indices + virtual int getNumSubParts() const = 0; + + 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 + { + (void)aabbMin; + (void)aabbMax; + } + virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const + { + (void)aabbMin; + (void)aabbMax; + } + + const btVector3& getScaling() const + { + return m_scaling; + } + void setScaling(const btVector3& scaling) + { + m_scaling = scaling; + } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; -struct btIntIndexData +struct btIntIndexData { - int m_value; + int m_value; }; -struct btShortIntIndexData +struct btShortIntIndexData { short m_value; char m_pad[2]; }; -struct btShortIntIndexTripletData +struct btShortIntIndexTripletData { - short m_values[3]; - char m_pad[2]; + short m_values[3]; + char m_pad[2]; }; -struct btCharIndexTripletData +struct btCharIndexTripletData { unsigned char m_values[3]; - char m_pad; + char m_pad; }; +// clang-format off ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btMeshPartData @@ -151,14 +143,11 @@ struct btStridingMeshInterfaceData char m_padding[4]; }; +// clang-format on - - -SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const { return sizeof(btStridingMeshInterfaceData); } - - -#endif //BT_STRIDING_MESHINTERFACE_H +#endif //BT_STRIDING_MESHINTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 52f346bf726..c4d33c429f0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -16,29 +16,29 @@ subject to the following restrictions: #include "btTetrahedronShape.h" #include "LinearMath/btMatrix3x3.h" -btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); addVertex(pt1); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); @@ -46,8 +46,8 @@ m_numVertices(0) addVertex(pt2); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2, const btVector3& pt3) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); @@ -56,17 +56,16 @@ m_numVertices(0) addVertex(pt3); } - -void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBU_Simplex1to4::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { #if 1 - btPolyhedralConvexAabbCachingShape::getAabb(t,aabbMin,aabbMax); + btPolyhedralConvexAabbCachingShape::getAabb(t, aabbMin, aabbMax); #else - aabbMin.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); - aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); + aabbMin.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); + aabbMax.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT); //just transform the vertices in worldspace, and take their AABB - for (int i=0;i<m_numVertices;i++) + for (int i = 0; i < m_numVertices; i++) { btVector3 worldVertex = t(m_vertices[i]); aabbMin.setMin(worldVertex); @@ -75,18 +74,13 @@ void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3 #endif } - - - - void btBU_Simplex1to4::addVertex(const btVector3& pt) { m_vertices[m_numVertices++] = pt; recalcLocalAabb(); } - -int btBU_Simplex1to4::getNumVertices() const +int btBU_Simplex1to4::getNumVertices() const { return m_numVertices; } @@ -97,122 +91,113 @@ int btBU_Simplex1to4::getNumEdges() const switch (m_numVertices) { - case 0: - return 0; - case 1: return 0; - case 2: return 1; - case 3: return 3; - case 4: return 6; - - + case 0: + return 0; + case 1: + return 0; + case 2: + return 1; + case 3: + return 3; + case 4: + return 6; } return 0; } -void btBU_Simplex1to4::getEdge(int i,btVector3& pa,btVector3& pb) const +void btBU_Simplex1to4::getEdge(int i, btVector3& pa, btVector3& pb) const { - - switch (m_numVertices) + switch (m_numVertices) { - - case 2: - pa = m_vertices[0]; - pb = m_vertices[1]; - break; - case 3: - switch (i) - { - case 0: - pa = m_vertices[0]; - pb = m_vertices[1]; - break; - case 1: - pa = m_vertices[1]; - pb = m_vertices[2]; - break; case 2: - pa = m_vertices[2]; - pb = m_vertices[0]; - break; - - } - break; - case 4: - switch (i) - { - case 0: pa = m_vertices[0]; pb = m_vertices[1]; break; - case 1: - pa = m_vertices[1]; - pb = m_vertices[2]; - break; - case 2: - pa = m_vertices[2]; - pb = m_vertices[0]; - break; case 3: - pa = m_vertices[0]; - pb = m_vertices[3]; + switch (i) + { + case 0: + pa = m_vertices[0]; + pb = m_vertices[1]; + break; + case 1: + pa = m_vertices[1]; + pb = m_vertices[2]; + break; + case 2: + pa = m_vertices[2]; + pb = m_vertices[0]; + break; + } break; case 4: - pa = m_vertices[1]; - pb = m_vertices[3]; - break; - case 5: - pa = m_vertices[2]; - pb = m_vertices[3]; - break; - } - + switch (i) + { + case 0: + pa = m_vertices[0]; + pb = m_vertices[1]; + break; + case 1: + pa = m_vertices[1]; + pb = m_vertices[2]; + break; + case 2: + pa = m_vertices[2]; + pb = m_vertices[0]; + break; + case 3: + pa = m_vertices[0]; + pb = m_vertices[3]; + break; + case 4: + pa = m_vertices[1]; + pb = m_vertices[3]; + break; + case 5: + pa = m_vertices[2]; + pb = m_vertices[3]; + break; + } } - - - - } -void btBU_Simplex1to4::getVertex(int i,btVector3& vtx) const +void btBU_Simplex1to4::getVertex(int i, btVector3& vtx) const { vtx = m_vertices[i]; } -int btBU_Simplex1to4::getNumPlanes() const +int btBU_Simplex1to4::getNumPlanes() const { switch (m_numVertices) { - case 0: + case 0: return 0; - case 1: + case 1: return 0; - case 2: + case 2: return 0; - case 3: + case 3: return 2; - case 4: + case 4: return 4; - default: + default: { } } return 0; } - -void btBU_Simplex1to4::getPlane(btVector3&, btVector3& ,int ) const +void btBU_Simplex1to4::getPlane(btVector3&, btVector3&, int) const { - } -int btBU_Simplex1to4::getIndex(int ) const +int btBU_Simplex1to4::getIndex(int) const { return 0; } -bool btBU_Simplex1to4::isInside(const btVector3& ,btScalar ) const +bool btBU_Simplex1to4::isInside(const btVector3&, btScalar) const { return false; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h index b69209835ef..f5e2209ed09 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -16,61 +16,56 @@ subject to the following restrictions: #ifndef BT_SIMPLEX_1TO4_SHAPE #define BT_SIMPLEX_1TO4_SHAPE - #include "btPolyhedralConvexShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" - ///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead. -ATTRIBUTE_ALIGNED16(class) btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape { protected: - - int m_numVertices; - btVector3 m_vertices[4]; + int m_numVertices; + btVector3 m_vertices[4]; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btBU_Simplex1to4(); btBU_Simplex1to4(const btVector3& pt0); - btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1); - btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2); - btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3); + btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1); + btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2); + btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2, const btVector3& pt3); - - void reset() + void reset() { m_numVertices = 0; } - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; void addVertex(const btVector3& pt); //PolyhedralConvexShape interface - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - - virtual void getVertex(int i,btVector3& vtx) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; - virtual int getNumPlanes() const; + virtual void getVertex(int i, btVector3& vtx) const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const; + virtual int getNumPlanes() const; - virtual int getIndex(int i) const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual int getIndex(int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///getName is for debugging - virtual const char* getName()const { return "btBU_Simplex1to4";} - + virtual const char* getName() const { return "btBU_Simplex1to4"; } }; -#endif //BT_SIMPLEX_1TO4_SHAPE +#endif //BT_SIMPLEX_1TO4_SHAPE diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp index 3027e65b256..3b6db2b39fa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp @@ -15,21 +15,14 @@ subject to the following restrictions: #include "btTriangleBuffer.h" - - - - - - -void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex) +void btTriangleBuffer::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangle tri; - tri.m_vertex0 = triangle[0]; - tri.m_vertex1 = triangle[1]; - tri.m_vertex2 = triangle[2]; - tri.m_partId = partId; - tri.m_triangleIndex = triangleIndex; - - m_triangleBuffer.push_back(tri); + btTriangle tri; + tri.m_vertex0 = triangle[0]; + tri.m_vertex1 = triangle[1]; + tri.m_vertex2 = triangle[2]; + tri.m_partId = partId; + tri.m_triangleIndex = triangleIndex; + + m_triangleBuffer.push_back(tri); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index b71fc8b3746..a89b9cd8a48 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -19,13 +19,13 @@ subject to the following restrictions: #include "btTriangleCallback.h" #include "LinearMath/btAlignedObjectArray.h" -struct btTriangle +struct btTriangle { - btVector3 m_vertex0; - btVector3 m_vertex1; - btVector3 m_vertex2; - int m_partId; - int m_triangleIndex; + btVector3 m_vertex0; + btVector3 m_vertex1; + btVector3 m_vertex2; + int m_partId; + int m_triangleIndex; }; ///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' @@ -39,31 +39,25 @@ struct btTriangle /// } class btTriangleBuffer : public btTriangleCallback { + btAlignedObjectArray<btTriangle> m_triangleBuffer; - btAlignedObjectArray<btTriangle> m_triangleBuffer; - public: - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - int getNumTriangles() const + + int getNumTriangles() const { return int(m_triangleBuffer.size()); } - - const btTriangle& getTriangle(int index) const + + const btTriangle& getTriangle(int index) const { return m_triangleBuffer[index]; } - void clearBuffer() + void clearBuffer() { m_triangleBuffer.clear(); } - }; - -#endif //BT_TRIANGLE_BUFFER_H - +#endif //BT_TRIANGLE_BUFFER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp index f558bf6d241..5bd2c595fed 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp @@ -17,12 +17,8 @@ subject to the following restrictions: btTriangleCallback::~btTriangleCallback() { - } - btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback() { - } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h index 461c57f8773..d3644891ee6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -18,13 +18,11 @@ subject to the following restrictions: #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: - virtual ~btTriangleCallback(); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) = 0; }; @@ -32,11 +30,8 @@ public: class btInternalTriangleIndexCallback { public: - virtual ~btInternalTriangleIndexCallback(); - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) = 0; + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) = 0; }; - - -#endif //BT_TRIANGLE_CALLBACK_H +#endif //BT_TRIANGLE_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index a665024cb61..dae42551941 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -15,81 +15,76 @@ 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) +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_triangleIndexBase = (const unsigned char*)triangleIndexBase; mesh.m_triangleIndexStride = triangleIndexStride; mesh.m_numVertices = numVertices; - mesh.m_vertexBase = (const unsigned char *)vertexBase; + 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) +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() ); + btAssert(subpart < getNumSubParts()); btIndexedMesh& mesh = m_indexedMeshes[subpart]; numverts = mesh.m_numVertices; - (*vertexbase) = (unsigned char *) mesh.m_vertexBase; + (*vertexbase) = (unsigned char*)mesh.m_vertexBase; - type = mesh.m_vertexType; + type = mesh.m_vertexType; vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; - (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; + (*indexbase) = (unsigned char*)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; 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 +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 { const btIndexedMesh& mesh = m_indexedMeshes[subpart]; numverts = mesh.m_numVertices; - (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; + (*vertexbase) = (const unsigned char*)mesh.m_vertexBase; + + type = mesh.m_vertexType; - type = mesh.m_vertexType; - vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; - (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; + (*indexbase) = (const unsigned char*)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; indicestype = mesh.m_indexType; } -bool btTriangleIndexVertexArray::hasPremadeAabb() const +bool btTriangleIndexVertexArray::hasPremadeAabb() const { return (m_hasAabb == 1); } - -void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const +void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const { m_aabbMin = aabbMin; m_aabbMax = aabbMax; - m_hasAabb = 1; // this is intentionally an int see notes in header + m_hasAabb = 1; // this is intentionally an int see notes in header } -void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const +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 9e1544e87a4..556aa3fef4d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -20,62 +20,59 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btScalar.h" - ///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 +ATTRIBUTE_ALIGNED16(struct) +btIndexedMesh { BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_numTriangles; - const unsigned char * m_triangleIndexBase; - // Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed) - int m_triangleIndexStride; - int m_numVertices; - const unsigned char * m_vertexBase; - // Size of a vertex, in bytes - int m_vertexStride; - - // The index type is set when adding an indexed mesh to the - // btTriangleIndexVertexArray, do not set it manually - PHY_ScalarType m_indexType; - - // The vertex type has a default type similar to Bullet's precision mode (float or double) - // but can be set manually if you for example run Bullet with double precision but have - // mesh data in single precision.. - PHY_ScalarType m_vertexType; - - - btIndexedMesh() - :m_indexType(PHY_INTEGER), + int m_numTriangles; + const unsigned char* m_triangleIndexBase; + // Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed) + int m_triangleIndexStride; + int m_numVertices; + const unsigned char* m_vertexBase; + // Size of a vertex, in bytes + int m_vertexStride; + + // The index type is set when adding an indexed mesh to the + // btTriangleIndexVertexArray, do not set it manually + PHY_ScalarType m_indexType; + + // The vertex type has a default type similar to Bullet's precision mode (float or double) + // but can be set manually if you for example run Bullet with double precision but have + // mesh data in single precision.. + PHY_ScalarType m_vertexType; + + btIndexedMesh() + : m_indexType(PHY_INTEGER), #ifdef BT_USE_DOUBLE_PRECISION - m_vertexType(PHY_DOUBLE) -#else // BT_USE_DOUBLE_PRECISION - m_vertexType(PHY_FLOAT) -#endif // BT_USE_DOUBLE_PRECISION - { - } -} -; - + m_vertexType(PHY_DOUBLE) +#else // BT_USE_DOUBLE_PRECISION + m_vertexType(PHY_FLOAT) +#endif // BT_USE_DOUBLE_PRECISION + { + } +}; -typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray; +typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray; ///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. +///No duplicate 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 +ATTRIBUTE_ALIGNED16(class) +btTriangleIndexVertexArray : public btStridingMeshInterface { protected: - IndexedMeshArray m_indexedMeshes; + IndexedMeshArray m_indexedMeshes; int m_pad[2]; - mutable int m_hasAabb; // using int instead of bool to maintain alignment + mutable int m_hasAabb; // using int instead of bool to maintain alignment mutable btVector3 m_aabbMin; mutable btVector3 m_aabbMax; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btTriangleIndexVertexArray() : m_hasAabb(0) @@ -85,49 +82,47 @@ public: virtual ~btTriangleIndexVertexArray(); //just to be backwards compatible - btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); - - void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) + btTriangleIndexVertexArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, int numVertices, btScalar* vertexBase, int vertexStride); + + void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) { m_indexedMeshes.push_back(mesh); - m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType; + m_indexedMeshes[m_indexedMeshes.size() - 1].m_indexType = indexType; } - - - virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,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& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const; + virtual void getLockedVertexIndexBase(unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& vertexStride, 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& vertexStride, 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 unLockVertexBase(int subpart) { (void)subpart; } - virtual void unLockReadOnlyVertexBase(int subpart) const {(void)subpart;} + virtual void unLockReadOnlyVertexBase(int subpart) const { (void)subpart; } - /// getNumSubParts returns the number of seperate subparts + /// getNumSubParts returns the number of separate subparts /// each subpart has a continuous array of vertices and indices - virtual int getNumSubParts() const { + virtual int getNumSubParts() const + { return (int)m_indexedMeshes.size(); } - IndexedMeshArray& getIndexedMeshArray() + IndexedMeshArray& getIndexedMeshArray() { return m_indexedMeshes; } - const IndexedMeshArray& getIndexedMeshArray() const + const IndexedMeshArray& getIndexedMeshArray() const { return m_indexedMeshes; } - 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 ) const; - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const; + 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) const; + virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const; +}; -#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H +#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp index dc562941ad6..4bf133d7ac7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -17,70 +17,68 @@ subject to the following restrictions: #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) +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; + btMaterialProperties mat; - mat.m_numMaterials = numMaterials; - mat.m_materialBase = materialBase; - mat.m_materialStride = materialStride; + mat.m_numMaterials = numMaterials; + mat.m_materialBase = materialBase; + mat.m_materialStride = materialStride; #ifdef BT_USE_DOUBLE_PRECISION - mat.m_materialType = PHY_DOUBLE; + mat.m_materialType = PHY_DOUBLE; #else - mat.m_materialType = PHY_FLOAT; + 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; + mat.m_numTriangles = numTriangles; + mat.m_triangleMaterialsBase = (unsigned char*)triangleMaterialsBase; + mat.m_triangleMaterialStride = materialIndexStride; + mat.m_triangleType = PHY_INTEGER; - addMaterialProperties(mat); + 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) +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() ); + btAssert(subpart < getNumSubParts()); - btMaterialProperties& mats = m_materials[subpart]; + btMaterialProperties& mats = m_materials[subpart]; - numMaterials = mats.m_numMaterials; - (*materialBase) = (unsigned char *) mats.m_materialBase; + numMaterials = mats.m_numMaterials; + (*materialBase) = (unsigned char*)mats.m_materialBase; #ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; + materialType = PHY_DOUBLE; #else - materialType = PHY_FLOAT; + materialType = PHY_FLOAT; #endif - materialStride = mats.m_materialStride; + materialStride = mats.m_materialStride; - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; + 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) +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]; + btMaterialProperties& mats = m_materials[subpart]; - numMaterials = mats.m_numMaterials; - (*materialBase) = (const unsigned char *) mats.m_materialBase; + numMaterials = mats.m_numMaterials; + (*materialBase) = (const unsigned char*)mats.m_materialBase; #ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; + materialType = PHY_DOUBLE; #else - materialType = PHY_FLOAT; + materialType = PHY_FLOAT; #endif - materialStride = mats.m_materialStride; + materialStride = mats.m_materialStride; - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; + 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 index ba4f7b46076..315b1e21f35 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -20,26 +20,26 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" - -ATTRIBUTE_ALIGNED16( struct) btMaterialProperties +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; + ///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; +typedef btAlignedObjectArray<btMaterialProperties> MaterialArray; ///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray ///The addition of a material array allows for the utilization of the partID and @@ -47,38 +47,37 @@ typedef btAlignedObjectArray<btMaterialProperties> MaterialArray; ///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 +ATTRIBUTE_ALIGNED16(class) +btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray { protected: - MaterialArray m_materials; - + MaterialArray m_materials; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btTriangleIndexVertexMaterialArray() + 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() {} + btTriangleIndexVertexMaterialArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, + int numVertices, btScalar* vertexBase, int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride); - 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 ~btTriangleIndexVertexMaterialArray() {} - 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); + 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 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); + 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 +#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h index 17deef89d37..8ee35ef5faf 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -16,11 +16,9 @@ subject to the following restrictions: #ifndef _BT_TRIANGLE_INFO_MAP_H #define _BT_TRIANGLE_INFO_MAP_H - #include "LinearMath/btHashMap.h" #include "LinearMath/btSerializer.h" - ///for btTriangleInfo m_flags #define TRI_INFO_V0V1_CONVEX 1 #define TRI_INFO_V1V2_CONVEX 2 @@ -30,61 +28,58 @@ subject to the following restrictions: #define TRI_INFO_V1V2_SWAP_NORMALB 16 #define TRI_INFO_V2V0_SWAP_NORMALB 32 - ///The btTriangleInfo structure stores information to adjust collision normals to avoid collisions against internal edges -///it can be generated using -struct btTriangleInfo +///it can be generated using +struct btTriangleInfo { btTriangleInfo() { m_edgeV0V1Angle = SIMD_2_PI; m_edgeV1V2Angle = SIMD_2_PI; m_edgeV2V0Angle = SIMD_2_PI; - m_flags=0; + m_flags = 0; } - int m_flags; - - btScalar m_edgeV0V1Angle; - btScalar m_edgeV1V2Angle; - btScalar m_edgeV2V0Angle; + int m_flags; + btScalar m_edgeV0V1Angle; + btScalar m_edgeV1V2Angle; + btScalar m_edgeV2V0Angle; }; -typedef btHashMap<btHashInt,btTriangleInfo> btInternalTriangleInfoMap; - +typedef btHashMap<btHashInt, btTriangleInfo> btInternalTriangleInfoMap; ///The btTriangleInfoMap stores edge angle information for some triangles. You can compute this information yourself or using btGenerateInternalEdgeInfo. -struct btTriangleInfoMap : public btInternalTriangleInfoMap +struct btTriangleInfoMap : public btInternalTriangleInfoMap { - btScalar m_convexEpsilon;///used to determine if an edge or contact normal is convex, using the dot product - btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle - btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' - btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" - btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold - btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) - - + btScalar m_convexEpsilon; ///used to determine if an edge or contact normal is convex, using the dot product + btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle + btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' + btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" + btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold + btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) + btTriangleInfoMap() { m_convexEpsilon = 0.00f; m_planarEpsilon = 0.0001f; - m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001); + m_equalVertexThreshold = btScalar(0.0001) * btScalar(0.0001); m_edgeDistanceThreshold = btScalar(0.1); - m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001); + m_zeroAreaThreshold = btScalar(0.0001) * btScalar(0.0001); m_maxEdgeAngleThreshold = SIMD_2_PI; } virtual ~btTriangleInfoMap() {} - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - void deSerialize(struct btTriangleInfoMapData& data); + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + void deSerialize(struct btTriangleInfoMapData& data); }; +// clang-format off + ///those fields have to be float and not btScalar for the serialization to work properly struct btTriangleInfoData { @@ -114,128 +109,130 @@ struct btTriangleInfoMapData char m_padding[4]; }; -SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const +// clang-format on + +SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const { return sizeof(btTriangleInfoMapData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const { - btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*) dataBuffer; + btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*)dataBuffer; tmapData->m_convexEpsilon = (float)m_convexEpsilon; tmapData->m_planarEpsilon = (float)m_planarEpsilon; - tmapData->m_equalVertexThreshold =(float) m_equalVertexThreshold; + tmapData->m_equalVertexThreshold = (float)m_equalVertexThreshold; tmapData->m_edgeDistanceThreshold = (float)m_edgeDistanceThreshold; tmapData->m_zeroAreaThreshold = (float)m_zeroAreaThreshold; - + tmapData->m_hashTableSize = m_hashTable.size(); tmapData->m_hashTablePtr = tmapData->m_hashTableSize ? (int*)serializer->getUniquePointer((void*)&m_hashTable[0]) : 0; if (tmapData->m_hashTablePtr) - { + { //serialize an int buffer int sz = sizeof(int); int numElem = tmapData->m_hashTableSize; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_hashTable[i]; } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_hashTable[0]); - + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_hashTable[0]); } tmapData->m_nextSize = m_next.size(); - tmapData->m_nextPtr = tmapData->m_nextSize? (int*)serializer->getUniquePointer((void*)&m_next[0]): 0; + tmapData->m_nextPtr = tmapData->m_nextSize ? (int*)serializer->getUniquePointer((void*)&m_next[0]) : 0; if (tmapData->m_nextPtr) { int sz = sizeof(int); int numElem = tmapData->m_nextSize; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_next[i]; } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_next[0]); + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_next[0]); } - + tmapData->m_numValues = m_valueArray.size(); - tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]): 0; + tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]) : 0; if (tmapData->m_valueArrayPtr) { int sz = sizeof(btTriangleInfoData); int numElem = tmapData->m_numValues; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btTriangleInfoData* memPtr = (btTriangleInfoData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_edgeV0V1Angle = (float)m_valueArray[i].m_edgeV0V1Angle; memPtr->m_edgeV1V2Angle = (float)m_valueArray[i].m_edgeV1V2Angle; memPtr->m_edgeV2V0Angle = (float)m_valueArray[i].m_edgeV2V0Angle; memPtr->m_flags = m_valueArray[i].m_flags; } - serializer->finalizeChunk(chunk,"btTriangleInfoData",BT_ARRAY_CODE,(void*) &m_valueArray[0]); + serializer->finalizeChunk(chunk, "btTriangleInfoData", BT_ARRAY_CODE, (void*)&m_valueArray[0]); } - + tmapData->m_numKeys = m_keyArray.size(); tmapData->m_keyArrayPtr = tmapData->m_numKeys ? (int*)serializer->getUniquePointer((void*)&m_keyArray[0]) : 0; if (tmapData->m_keyArrayPtr) { int sz = sizeof(int); int numElem = tmapData->m_numValues; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_keyArray[i].getUid1(); } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]); - + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_keyArray[0]); } - return "btTriangleInfoMapData"; -} + // Fill padding with zeros to appease msan. + tmapData->m_padding[0] = 0; + tmapData->m_padding[1] = 0; + tmapData->m_padding[2] = 0; + tmapData->m_padding[3] = 0; + return "btTriangleInfoMapData"; +} ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData ) +SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData) { - - m_convexEpsilon = tmapData.m_convexEpsilon; m_planarEpsilon = tmapData.m_planarEpsilon; m_equalVertexThreshold = tmapData.m_equalVertexThreshold; m_edgeDistanceThreshold = tmapData.m_edgeDistanceThreshold; m_zeroAreaThreshold = tmapData.m_zeroAreaThreshold; m_hashTable.resize(tmapData.m_hashTableSize); - int i =0; - for (i=0;i<tmapData.m_hashTableSize;i++) + int i = 0; + for (i = 0; i < tmapData.m_hashTableSize; i++) { m_hashTable[i] = tmapData.m_hashTablePtr[i]; } m_next.resize(tmapData.m_nextSize); - for (i=0;i<tmapData.m_nextSize;i++) + for (i = 0; i < tmapData.m_nextSize; i++) { m_next[i] = tmapData.m_nextPtr[i]; } m_valueArray.resize(tmapData.m_numValues); - for (i=0;i<tmapData.m_numValues;i++) + for (i = 0; i < tmapData.m_numValues; i++) { m_valueArray[i].m_edgeV0V1Angle = tmapData.m_valueArrayPtr[i].m_edgeV0V1Angle; m_valueArray[i].m_edgeV1V2Angle = tmapData.m_valueArrayPtr[i].m_edgeV1V2Angle; m_valueArray[i].m_edgeV2V0Angle = tmapData.m_valueArrayPtr[i].m_edgeV2V0Angle; m_valueArray[i].m_flags = tmapData.m_valueArrayPtr[i].m_flags; } - - m_keyArray.resize(tmapData.m_numKeys,btHashInt(0)); - for (i=0;i<tmapData.m_numKeys;i++) + + m_keyArray.resize(tmapData.m_numKeys, btHashInt(0)); + for (i = 0; i < tmapData.m_numKeys; i++) { m_keyArray[i].setUid1(tmapData.m_keyArrayPtr[i]); } } - -#endif //_BT_TRIANGLE_INFO_MAP_H +#endif //_BT_TRIANGLE_INFO_MAP_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index e4de7320930..abd8c227866 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -13,38 +13,36 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btTriangleMesh.h" - - -btTriangleMesh::btTriangleMesh (bool use32bitIndices,bool use4componentVertices) -:m_use32bitIndices(use32bitIndices), -m_use4componentVertices(use4componentVertices), -m_weldingThreshold(0.0) +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_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_numTriangles = m_32bitIndices.size() / 3; m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_INTEGER; - m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int); - } else + m_indexedMeshes[0].m_triangleIndexStride = 3 * sizeof(int); + } + else { - m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3; + m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size() / 3; m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_SHORT; - m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int); + m_indexedMeshes[0].m_triangleIndexStride = 3 * sizeof(short int); } if (m_use4componentVertices) @@ -52,48 +50,48 @@ m_weldingThreshold(0.0) m_indexedMeshes[0].m_numVertices = m_4componentVertices.size(); m_indexedMeshes[0].m_vertexBase = 0; m_indexedMeshes[0].m_vertexStride = sizeof(btVector3); - } else + } + else { - m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3; + m_indexedMeshes[0].m_numVertices = m_3componentVertices.size() / 3; m_indexedMeshes[0].m_vertexBase = 0; - m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar); + m_indexedMeshes[0].m_vertexStride = 3 * sizeof(btScalar); } - - } -void btTriangleMesh::addIndex(int index) +void btTriangleMesh::addIndex(int index) { if (m_use32bitIndices) { m_32bitIndices.push_back(index); - m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0]; - } else + 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]; + m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*)&m_16bitIndices[0]; } } -void btTriangleMesh::addTriangleIndices(int index1, int index2, int index3 ) +void btTriangleMesh::addTriangleIndices(int index1, int index2, int index3) { m_indexedMeshes[0].m_numTriangles++; - addIndex( index1 ); - addIndex( index2 ); - addIndex( index3 ); + addIndex(index1); + addIndex(index2); + addIndex(index3); } -int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices) +int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices) { //return index of new/existing vertex ///@todo: could use acceleration structure for this if (m_use4componentVertices) { if (removeDuplicateVertices) + { + for (int i = 0; i < m_4componentVertices.size(); i++) { - for (int i=0;i< m_4componentVertices.size();i++) - { - if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold) + if ((m_4componentVertices[i] - vertex).length2() <= m_weldingThreshold) { return i; } @@ -103,19 +101,18 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat m_4componentVertices.push_back(vertex); m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0]; - return m_4componentVertices.size()-1; - - } else + return m_4componentVertices.size() - 1; + } + else { - if (removeDuplicateVertices) { - for (int i=0;i< m_3componentVertices.size();i+=3) + 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) + btVector3 vtx(m_3componentVertices[i], m_3componentVertices[i + 1], m_3componentVertices[i + 2]); + if ((vtx - vertex).length2() <= m_weldingThreshold) { - return i/3; + return i / 3; } } } @@ -124,17 +121,16 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat 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; + return (m_3componentVertices.size() / 3) - 1; } - } - -void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2,bool removeDuplicateVertices) + +void btTriangleMesh::addTriangle(const btVector3& vertex0, const btVector3& vertex1, const btVector3& vertex2, bool removeDuplicateVertices) { m_indexedMeshes[0].m_numTriangles++; - addIndex(findOrAddVertex(vertex0,removeDuplicateVertices)); - addIndex(findOrAddVertex(vertex1,removeDuplicateVertices)); - addIndex(findOrAddVertex(vertex2,removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex0, removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex1, removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex2, removeDuplicateVertices)); } int btTriangleMesh::getNumTriangles() const @@ -151,7 +147,8 @@ void btTriangleMesh::preallocateVertices(int numverts) if (m_use4componentVertices) { m_4componentVertices.reserve(numverts); - } else + } + else { m_3componentVertices.reserve(numverts); } @@ -162,7 +159,8 @@ void btTriangleMesh::preallocateIndices(int numindices) if (m_use32bitIndices) { m_32bitIndices.reserve(numindices); - } else + } + else { m_16bitIndices.reserve(numindices); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index ac4afa7f6b2..a8a362355cc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -26,47 +26,44 @@ subject to the following restrictions: ///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same. class btTriangleMesh : public btTriangleIndexVertexArray { - btAlignedObjectArray<btVector3> m_4componentVertices; - btAlignedObjectArray<btScalar> m_3componentVertices; + btAlignedObjectArray<btVector3> m_4componentVertices; + btAlignedObjectArray<btScalar> m_3componentVertices; - btAlignedObjectArray<unsigned int> m_32bitIndices; - btAlignedObjectArray<unsigned short int> m_16bitIndices; - bool m_use32bitIndices; - bool m_use4componentVertices; - + btAlignedObjectArray<unsigned int> m_32bitIndices; + btAlignedObjectArray<unsigned short int> m_16bitIndices; + bool m_use32bitIndices; + bool m_use4componentVertices; - public: - btScalar m_weldingThreshold; +public: + btScalar m_weldingThreshold; - btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); + btTriangleMesh(bool use32bitIndices = true, bool use4componentVertices = true); - bool getUse32bitIndices() const - { - return m_use32bitIndices; - } + bool getUse32bitIndices() const + { + return m_use32bitIndices; + } - bool getUse4componentVertices() const - { - return m_use4componentVertices; - } - ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes. - ///In general it is better to directly use btTriangleIndexVertexArray instead. - void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false); + bool getUse4componentVertices() const + { + return m_use4componentVertices; + } + ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes. + ///In general it is better to directly use btTriangleIndexVertexArray instead. + void addTriangle(const btVector3& vertex0, const btVector3& vertex1, const btVector3& vertex2, bool removeDuplicateVertices = false); - ///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices) - void addTriangleIndices(int index1, int index2, int index3 ); - - int getNumTriangles() const; + ///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices) + void addTriangleIndices(int index1, int index2, int index3); - virtual void preallocateVertices(int numverts); - virtual void preallocateIndices(int numindices); + int getNumTriangles() const; - ///findOrAddVertex is an internal method, use addTriangle instead - int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); - ///addIndex is an internal method, use addTriangle instead - void addIndex(int index); - -}; + virtual void preallocateVertices(int numverts); + virtual void preallocateIndices(int numindices); -#endif //BT_TRIANGLE_MESH_H + ///findOrAddVertex is an internal method, use addTriangle instead + int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); + ///addIndex is an internal method, use addTriangle instead + void addIndex(int index); +}; +#endif //BT_TRIANGLE_MESH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index 0e17951405c..aec239063cd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -20,12 +20,11 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" - btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) -: btConcaveShape (), m_meshInterface(meshInterface) + : btConcaveShape(), m_meshInterface(meshInterface) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; - if(meshInterface->hasPremadeAabb()) + if (meshInterface->hasPremadeAabb()) { meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax); } @@ -35,69 +34,60 @@ btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) } } - btTriangleMeshShape::~btTriangleMeshShape() { - } - - - -void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btTriangleMeshShape::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); - 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(); + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); aabbMin = center - extent; aabbMax = center + extent; } -void btTriangleMeshShape::recalcLocalAabb() +void btTriangleMeshShape::recalcLocalAabb() { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i] + m_collisionMargin; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i] - m_collisionMargin; } } - - class SupportVertexCallback : public btTriangleCallback { - btVector3 m_supportVertexLocal; -public: - btTransform m_worldTrans; +public: + btTransform m_worldTrans; btScalar m_maxDot; btVector3 m_supportVecLocal; - SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-BT_LARGE_FLOAT)) - + SupportVertexCallback(const btVector3& supportVecWorld, const btTransform& trans) + : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)), m_worldTrans(trans), m_maxDot(btScalar(-BT_LARGE_FLOAT)) + { m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); } - virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex) + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; (void)triangleIndex; - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); if (dot > m_maxDot) @@ -113,14 +103,12 @@ public: return m_worldTrans(m_supportVertexLocal); } - btVector3 GetSupportVertexLocal() + btVector3 GetSupportVertexLocal() { return m_supportVertexLocal; } - }; - void btTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_meshInterface->setScaling(scaling); @@ -132,60 +120,46 @@ const btVector3& btTriangleMeshShape::getLocalScaling() const return m_meshInterface->getScaling(); } - - - - - //#define DEBUG_TRIANGLE_MESH - - -void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - struct FilteredCallback : public btInternalTriangleIndexCallback + struct FilteredCallback : public btInternalTriangleIndexCallback { btTriangleCallback* m_callback; btVector3 m_aabbMin; btVector3 m_aabbMax; - FilteredCallback(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) - :m_callback(callback), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax) + FilteredCallback(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) + : m_callback(callback), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { - if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax)) + if (TestTriangleAgainstAabb2(&triangle[0], m_aabbMin, m_aabbMax)) { //check aabb in triangle-space, before doing this - m_callback->processTriangle(triangle,partId,triangleIndex); + m_callback->processTriangle(triangle, partId, triangleIndex); } - } - }; - FilteredCallback filterCallback(callback,aabbMin,aabbMax); + FilteredCallback filterCallback(callback, aabbMin, aabbMax); - m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax); + m_meshInterface->InternalProcessAllTriangles(&filterCallback, aabbMin, aabbMax); } - - - - -void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; //moving concave objects not supported btAssert(0); - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } - btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supportVertex; @@ -193,15 +167,13 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co btTransform ident; ident.setIdentity(); - SupportVertexCallback supportCallback(vec,ident); + SupportVertexCallback supportCallback(vec, ident); + + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + + processAllTriangles(&supportCallback, -aabbMax, aabbMax); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - - processAllTriangles(&supportCallback,-aabbMax,aabbMax); - supportVertex = supportCallback.GetSupportVertexLocal(); return supportVertex; } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 453e58005a4..4a70e283fae 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -19,18 +19,18 @@ subject to the following restrictions: #include "btConcaveShape.h" #include "btStridingMeshInterface.h" - ///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead. -ATTRIBUTE_ALIGNED16(class) btTriangleMeshShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btTriangleMeshShape : public btConcaveShape { protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; btStridingMeshInterface* m_meshInterface; ///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); + btTriangleMeshShape(btStridingMeshInterface * meshInterface); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -39,23 +39,23 @@ public: virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { btAssert(0); return localGetSupportingVertex(vec); } - void recalcLocalAabb(); + void recalcLocalAabb(); - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + 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 processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; - + btStridingMeshInterface* getMeshInterface() { return m_meshInterface; @@ -75,16 +75,8 @@ public: return m_localAabbMax; } - - //debugging - virtual const char* getName()const {return "TRIANGLEMESH";} - - - + virtual const char* getName() const { return "TRIANGLEMESH"; } }; - - - -#endif //BT_TRIANGLE_MESH_SHAPE_H +#endif //BT_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index a8a80f82fe2..190cbdae69e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -19,15 +19,13 @@ subject to the following restrictions: #include "btConvexShape.h" #include "btBoxShape.h" -ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btTriangleShape : public btPolyhedralConvexShape { - - public: + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_vertices1[3]; + btVector3 m_vertices1[3]; virtual int getNumVertices() const { @@ -43,7 +41,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return m_vertices1[index]; } - virtual void getVertex(int index,btVector3& vert) const + virtual void getVertex(int index, btVector3& vert) const { vert = m_vertices1[index]; } @@ -52,83 +50,79 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return 3; } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const { - getVertex(i,pa); - getVertex((i+1)%3,pb); + getVertex(i, pa); + getVertex((i + 1) % 3, pb); } - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { -// btAssert(0); - getAabbSlow(t,aabbMin,aabbMax); + // btAssert(0); + getAabbSlow(t, aabbMin, aabbMax); } - btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const + btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir) const { - btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); - return m_vertices1[dots.maxAxis()]; - + btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); + return m_vertices1[dots.maxAxis()]; } - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { const btVector3& dir = vectors[i]; - btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); - supportVerticesOut[i] = m_vertices1[dots.maxAxis()]; + btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); + supportVerticesOut[i] = m_vertices1[dots.maxAxis()]; } - } - btTriangleShape() : btPolyhedralConvexShape () - { + btTriangleShape() : btPolyhedralConvexShape() + { m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; } - btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape () - { + btTriangleShape(const btVector3& p0, const btVector3& p1, const btVector3& p2) : btPolyhedralConvexShape() + { m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; - m_vertices1[0] = p0; - m_vertices1[1] = p1; - m_vertices1[2] = p2; - } - + m_vertices1[0] = p0; + m_vertices1[1] = p1; + m_vertices1[2] = p2; + } - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const { - getPlaneEquation(i,planeNormal,planeSupport); + getPlaneEquation(i, planeNormal, planeSupport); } - virtual int getNumPlanes() const + virtual int getNumPlanes() const { return 1; } - void calcNormal(btVector3& normal) const + void calcNormal(btVector3 & normal) const { - normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]); + normal = (m_vertices1[1] - m_vertices1[0]).cross(m_vertices1[2] - m_vertices1[0]); normal.normalize(); } - virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const + virtual void getPlaneEquation(int i, btVector3& planeNormal, btVector3& planeSupport) const { (void)i; calcNormal(planeNormal); planeSupport = m_vertices1[0]; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const { (void)mass; btAssert(0); - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 normal; calcNormal(normal); @@ -140,45 +134,42 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { //inside check on edge-planes int i; - for (i=0;i<3;i++) + for (i = 0; i < 3; i++) { - btVector3 pa,pb; - getEdge(i,pa,pb); - btVector3 edge = pb-pa; + btVector3 pa, pb; + getEdge(i, pa, pb); + btVector3 edge = pb - pa; btVector3 edgeNormal = edge.cross(normal); edgeNormal.normalize(); - btScalar dist = pt.dot( edgeNormal); + btScalar dist = pt.dot(edgeNormal); btScalar edgeConst = pa.dot(edgeNormal); dist -= edgeConst; if (dist < -tolerance) return false; } - + return true; } return false; } - //debugging - virtual const char* getName()const - { - return "Triangle"; - } - - virtual int getNumPreferredPenetrationDirections() const - { - return 2; - } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const - { - calcNormal(penetrationVector); - if (index) - penetrationVector *= btScalar(-1.); - } + //debugging + virtual const char* getName() const + { + return "Triangle"; + } + virtual int getNumPreferredPenetrationDirections() const + { + return 2; + } + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + calcNormal(penetrationVector); + if (index) + penetrationVector *= btScalar(-1.); + } }; -#endif //BT_OBB_TRIANGLE_MINKOWSKI_H - +#endif //BT_OBB_TRIANGLE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp index b148bbd99a5..ed3cd2d2597 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -15,122 +15,112 @@ subject to the following restrictions: #include "btUniformScalingShape.h" -btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor): -btConvexShape (), m_childConvexShape(convexChildShape), -m_uniformScalingFactor(uniformScalingFactor) +btUniformScalingShape::btUniformScalingShape(btConvexShape* convexChildShape, btScalar uniformScalingFactor) : btConvexShape(), m_childConvexShape(convexChildShape), m_uniformScalingFactor(uniformScalingFactor) { m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE; } - + btUniformScalingShape::~btUniformScalingShape() { } - -btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { btVector3 tmpVertex; tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); - return tmpVertex*m_uniformScalingFactor; + return tmpVertex * m_uniformScalingFactor; } -void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors); int i; - for (i=0;i<numVectors;i++) + for (i = 0; i < numVectors; i++) { supportVerticesOut[i] = supportVerticesOut[i] * m_uniformScalingFactor; } } - -btVector3 btUniformScalingShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btUniformScalingShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 tmpVertex; tmpVertex = m_childConvexShape->localGetSupportingVertex(vec); - return tmpVertex*m_uniformScalingFactor; + return tmpVertex * m_uniformScalingFactor; } - -void btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +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); + 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& trans,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btUniformScalingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getAabbSlow(trans,aabbMin,aabbMax); - + getAabbSlow(trans, aabbMin, aabbMax); } -void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btUniformScalingShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { #if 1 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(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.)}; + + for (int i = 0; i < 6; i++) { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - - for (int i=0;i<6;i++) - { - _directions[i] = _directions[i]*t.getBasis(); + _directions[i] = _directions[i] * t.getBasis(); } - + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - btVector3 aabbMin1(0,0,0),aabbMax1(0,0,0); - for ( int i = 0; i < 3; ++i ) + btVector3 aabbMin1(0, 0, 0), aabbMax1(0, 0, 0); + + for (int i = 0; i < 3; ++i) { aabbMax1[i] = t(_supporting[i])[i]; aabbMin1[i] = t(_supporting[i + 3])[i]; } - btVector3 marginVec(getMargin(),getMargin(),getMargin()); - aabbMin = aabbMin1-marginVec; - aabbMax = aabbMax1+marginVec; - + btVector3 marginVec(getMargin(), getMargin(), getMargin()); + aabbMin = aabbMin1 - marginVec; + aabbMax = aabbMax1 + marginVec; + #else btScalar margin = getMargin(); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); - btVector3 sv = localGetSupportingVertex(vec*t.getBasis()); + btVector3 sv = localGetSupportingVertex(vec * t.getBasis()); btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; + aabbMax[i] = tmp[i] + margin; vec[i] = btScalar(-1.); - sv = localGetSupportingVertex(vec*t.getBasis()); + sv = localGetSupportingVertex(vec * t.getBasis()); tmp = t(sv); - aabbMin[i] = tmp[i]-margin; + aabbMin[i] = tmp[i] - margin; } #endif } -void btUniformScalingShape::setLocalScaling(const btVector3& scaling) +void btUniformScalingShape::setLocalScaling(const btVector3& scaling) { m_childConvexShape->setLocalScaling(scaling); } @@ -140,21 +130,21 @@ const btVector3& btUniformScalingShape::getLocalScaling() const return m_childConvexShape->getLocalScaling(); } -void btUniformScalingShape::setMargin(btScalar margin) +void btUniformScalingShape::setMargin(btScalar margin) { m_childConvexShape->setMargin(margin); } -btScalar btUniformScalingShape::getMargin() const +btScalar btUniformScalingShape::getMargin() const { return m_childConvexShape->getMargin() * m_uniformScalingFactor; } -int btUniformScalingShape::getNumPreferredPenetrationDirections() const +int btUniformScalingShape::getNumPreferredPenetrationDirections() const { return m_childConvexShape->getNumPreferredPenetrationDirections(); } - -void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + +void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); + m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h index a10f58d2429..4dfe34efbdc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -17,73 +17,68 @@ subject to the following restrictions: #define BT_UNIFORM_SCALING_SHAPE_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#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. -ATTRIBUTE_ALIGNED16(class) btUniformScalingShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btUniformScalingShape : public btConvexShape { - btConvexShape* m_childConvexShape; + btConvexShape* m_childConvexShape; - btScalar m_uniformScalingFactor; - - public: - + btScalar m_uniformScalingFactor; + +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btUniformScalingShape( btConvexShape* convexChildShape, btScalar uniformScalingFactor); - + + btUniformScalingShape(btConvexShape * convexChildShape, btScalar uniformScalingFactor); + virtual ~btUniformScalingShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + 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 batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - btScalar getUniformScalingFactor() const + btScalar getUniformScalingFactor() const { return m_uniformScalingFactor; } - btConvexShape* getChildShape() + btConvexShape* getChildShape() { return m_childConvexShape; } - const btConvexShape* getChildShape() const + const btConvexShape* getChildShape() const { return m_childConvexShape; } - virtual const char* getName()const + virtual const char* getName() const { return "UniformScalingShape"; } - - /////////////////////////// - ///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; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling) ; - virtual const btVector3& getLocalScaling() const ; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setMargin(btScalar margin); - virtual btScalar getMargin() const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; - virtual int getNumPreferredPenetrationDirections() const; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) 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 +#endif //BT_UNIFORM_SCALING_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h index 0a0357e5a81..182835c3b44 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h @@ -26,27 +26,21 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" - ///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) btFabs(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)) - - +#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) btFabs(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 { @@ -115,152 +109,144 @@ enum eBT_PLANE_INTERSECTION_TYPE // 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_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_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_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\ -}\ +#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) + const btMatrix3x3 &mat, const btVector3 &vec3, int colindex) { - return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][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 +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 + 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; + // 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(); - for(i=0;i<3;i++) - { - for(j=0;j<3;j++ ) - { - m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); - } - } + int i, j; + for (i = 0; i < 3; i++) + { + for (j = 0; j < 3; j++) + { + m_AR[i][j] = 1e-6f + btFabs(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) + 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) + 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_T1to0 += m_R1to0 * trans1.getOrigin(); m_R1to0 *= trans1.getBasis(); calc_absolute_matrix(); } - SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const + SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point) const { - return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0; + return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; } }; - #define BOX_PLANE_EPSILON 0.000001f //! Axis aligned box -ATTRIBUTE_ALIGNED16 (class) btAABB +ATTRIBUTE_ALIGNED16(class) +btAABB { public: btVector3 m_min; btVector3 m_max; btAABB() - {} - + { + } - btAABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3) + 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_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_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) + 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_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_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; @@ -270,13 +256,11 @@ public: m_max[2] += margin; } - btAABB(const btAABB &other): - m_min(other.m_min),m_max(other.m_max) + 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) + btAABB(const btAABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max) { m_min[0] -= margin; m_min[1] -= margin; @@ -317,34 +301,34 @@ public: m_max[2] = other.m_max[2] + margin; } - template<typename CLASS_POINT> + template <typename CLASS_POINT> SIMD_FORCE_INLINE void calc_from_triangle( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3) + 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_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_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> + 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) + 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_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_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; @@ -355,91 +339,89 @@ public: } //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + SIMD_FORCE_INLINE void appy_transform(const btTransform &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans(center); - btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), - trans.getBasis().getRow(1).absolute(), - trans.getBasis().getRow(2).absolute()); + btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), + trans.getBasis().getRow(1).absolute(), + 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) + SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans.transform(center); - btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), - trans.m_R1to0.getRow(1).absolute(), - trans.m_R1to0.getRow(2).absolute()); - + btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), + trans.m_R1to0.getRow(1).absolute(), + 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) + 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_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]); + 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) + 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_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]); + 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 + SIMD_FORCE_INLINE void get_center_extend(btVector3 & center, btVector3 & extend) const { - center = (m_max+m_min)*0.5f; + 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 + 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_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]); + 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 + 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]) + 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; } @@ -451,35 +433,34 @@ public: \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 + SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const { - btVector3 extents,center; - this->get_center_extend(center,extents);; + 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; + 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; + 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; - + 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; + 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; + 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; + 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 + 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; + btVector3 center = (m_max + m_min) * 0.5f; + btVector3 extend = m_max - center; - btScalar _fOrigin = direction.dot(center); + btScalar _fOrigin = direction.dot(center); btScalar _fMaximumExtent = extend.dot(direction.absolute()); vmin = _fOrigin - _fMaximumExtent; vmax = _fOrigin + _fMaximumExtent; @@ -487,30 +468,30 @@ public: SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const { - btScalar _fmin,_fmax; - this->projection_interval(plane,_fmin,_fmax); + btScalar _fmin, _fmax; + this->projection_interval(plane, _fmin, _fmax); - if(plane[3] > _fmax + BOX_PLANE_EPSILON) + if (plane[3] > _fmax + BOX_PLANE_EPSILON) { - return BT_CONST_BACK_PLANE; // 0 + return BT_CONST_BACK_PLANE; // 0 } - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + if (plane[3] + BOX_PLANE_EPSILON >= _fmin) { - return BT_CONST_COLLIDE_PLANE; //1 + return BT_CONST_COLLIDE_PLANE; //1 } - return BT_CONST_FRONT_PLANE;//2 + return BT_CONST_FRONT_PLANE; //2 } - SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const + 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 + 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); @@ -519,52 +500,50 @@ public: //! 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 + 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 ea, eb; //extends + btVector3 ca, cb; //extends + get_center_extend(ca, ea); + box.get_center_extend(cb, eb); btVector3 T; - btScalar t,t2; + btScalar t, t2; int i; // Class I : A's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[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; + if (BT_GREATER(T[i], t)) return false; } // Class II : B's basis vectors - for(i=0;i<3;i++) + 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; + 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) + if (fulltest) { - int j,m,n,o,p,q,r; - for(i=0;i<3;i++) + 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++) + 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; + 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; } } } @@ -573,7 +552,7 @@ public: //! Simple test for planes. SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) const + const btVector4 &plane) const { eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane); return (classify == BT_CONST_COLLIDE_PLANE); @@ -581,15 +560,15 @@ public: //! 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 + const btVector3 &p1, + const btVector3 &p2, + const btVector3 &p3, + const btVector4 &triangle_plane) const { - if(!collide_plane(triangle_plane)) return false; + if (!collide_plane(triangle_plane)) return false; - btVector3 center,extends; - this->get_center_extend(center,extends); + btVector3 center, extends; + this->get_center_extend(center, extends); const btVector3 v1(p1 - center); const btVector3 v2(p2 - center); @@ -599,47 +578,43 @@ public: 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_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_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); - + 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_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_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); + 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_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_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); + 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) +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2) { - if(!(t1.getOrigin() == t2.getOrigin()) ) return false; + 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; + 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 +#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h index de0a5231baf..38c23e222de 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h @@ -27,77 +27,74 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btGeometryUtil.h" - -SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point) +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) +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; + 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) + 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) + 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); + btScalar blendfactor = -dist0 / (dist1 - dist0); + bt_vec_blend(clipped[clipped_count], point0, point1, blendfactor); clipped_count++; } - if(!_classif) + 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) + const btVector4 &plane, + const btVector3 *polygon_points, + int polygon_point_count, + btVector3 *clipped) { - int clipped_count = 0; - + int clipped_count = 0; - //clip first point - btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);; - if(!(firstdist>SIMD_EPSILON)) + //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++) + for (int i = 1; i < polygon_point_count; i++) { - btScalar dist = bt_distance_point_plane(plane,polygon_points[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); - + polygon_points[i - 1], polygon_points[i], + olddist, + dist, + clipped, + clipped_count); olddist = dist; } @@ -105,11 +102,11 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon( //RETURN TO FIRST point bt_plane_clip_polygon_collect( - polygon_points[polygon_point_count-1],polygon_points[0], - olddist, - firstdist, - clipped, - clipped_count); + polygon_points[polygon_point_count - 1], polygon_points[0], + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } @@ -120,18 +117,19 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon( *\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 - ) + 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; + int clipped_count = 0; - //clip first point0 - btScalar firstdist = bt_distance_point_plane(plane,point0);; - if(!(firstdist>SIMD_EPSILON)) + //clip first point0 + btScalar firstdist = bt_distance_point_plane(plane, point0); + ; + if (!(firstdist > SIMD_EPSILON)) { clipped[clipped_count] = point0; clipped_count++; @@ -139,44 +137,37 @@ SIMD_FORCE_INLINE int bt_plane_clip_triangle( // point 1 btScalar olddist = firstdist; - btScalar dist = bt_distance_point_plane(plane,point1); + btScalar dist = bt_distance_point_plane(plane, point1); bt_plane_clip_polygon_collect( - point0,point1, - olddist, - dist, - clipped, - clipped_count); + point0, point1, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - // point 2 - dist = bt_distance_point_plane(plane,point2); + dist = bt_distance_point_plane(plane, point2); bt_plane_clip_polygon_collect( - point1,point2, - olddist, - dist, - clipped, - clipped_count); + 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); + point2, point0, + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } - - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h b/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h index 02f8b678a45..ede59e8a57f 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h @@ -5,89 +5,101 @@ #include "btGImpactShape.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +ATTRIBUTE_ALIGNED16(class) +btCompoundFromGimpactShape : public btCompoundShape +{ +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + virtual ~btCompoundFromGimpactShape() + { + /*delete all the btBU_Simplex1to4 ChildShapes*/ + for (int i = 0; i < m_children.size(); i++) + { + delete m_children[i].m_childShape; + } + } +}; + struct MyCallback : public btTriangleRaycastCallback +{ + int m_ignorePart; + int m_ignoreTriangleIndex; + + MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) + : btTriangleRaycastCallback(from, to), + m_ignorePart(ignorePart), + m_ignoreTriangleIndex(ignoreTriangleIndex) + { + } + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) + { + if (partId != m_ignorePart || triangleIndex != m_ignoreTriangleIndex) { - int m_ignorePart; - int m_ignoreTriangleIndex; - - - MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) - :btTriangleRaycastCallback(from,to), - m_ignorePart(ignorePart), - m_ignoreTriangleIndex(ignoreTriangleIndex) - { - - } - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) - { - if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex) - { - if (hitFraction < m_hitFraction) - return hitFraction; - } - - return m_hitFraction; - } - }; - struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback + if (hitFraction < m_hitFraction) + return hitFraction; + } + + return m_hitFraction; + } +}; +struct MyInternalTriangleIndexCallback : public btInternalTriangleIndexCallback +{ + const btGImpactMeshShape* m_gimpactShape; + btCompoundShape* m_colShape; + btScalar m_depth; + + MyInternalTriangleIndexCallback(btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) + : m_colShape(colShape), + m_gimpactShape(meshShape), + m_depth(depth) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + btVector3 scale = m_gimpactShape->getLocalScaling(); + btVector3 v0 = triangle[0] * scale; + btVector3 v1 = triangle[1] * scale; + btVector3 v2 = triangle[2] * scale; + + btVector3 centroid = (v0 + v1 + v2) / 3; + btVector3 normal = (v1 - v0).cross(v2 - v0); + normal.normalize(); + btVector3 rayFrom = centroid; + btVector3 rayTo = centroid - normal * m_depth; + + MyCallback cb(rayFrom, rayTo, partId, triangleIndex); + + m_gimpactShape->processAllTrianglesRay(&cb, rayFrom, rayTo); + if (cb.m_hitFraction < 1) { - const btGImpactMeshShape* m_gimpactShape; - btCompoundShape* m_colShape; - btScalar m_depth; - - MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) - :m_colShape(colShape), - m_gimpactShape(meshShape), - m_depth(depth) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - btVector3 scale = m_gimpactShape->getLocalScaling(); - btVector3 v0=triangle[0]*scale; - btVector3 v1=triangle[1]*scale; - btVector3 v2=triangle[2]*scale; - - btVector3 centroid = (v0+v1+v2)/3; - btVector3 normal = (v1-v0).cross(v2-v0); - normal.normalize(); - btVector3 rayFrom = centroid; - btVector3 rayTo = centroid-normal*m_depth; - - MyCallback cb(rayFrom,rayTo,partId,triangleIndex); - - m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo); - if (cb.m_hitFraction<1) - { - rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction); - //rayTo = cb.m_from; - //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); - //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); - } - - - - btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo); - btTransform ident; - ident.setIdentity(); - m_colShape->addChildShape(ident,tet); - } - }; - -btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) + rayTo.setInterpolate3(cb.m_from, cb.m_to, cb.m_hitFraction); + //rayTo = cb.m_from; + //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); + //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); + } + + btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0, v1, v2, rayTo); + btTransform ident; + ident.setIdentity(); + m_colShape->addChildShape(ident, tet); + } +}; + +btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) { - btCompoundShape* colShape = new btCompoundShape(); - - btTransform tr; - tr.setIdentity(); - - MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth); - btVector3 aabbMin,aabbMax; - gimpactMesh->getAabb(tr,aabbMin,aabbMax); - gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax); - - return colShape; -} - -#endif //BT_COMPOUND_FROM_GIMPACT
\ No newline at end of file + btCompoundShape* colShape = new btCompoundFromGimpactShape(); + + btTransform tr; + tr.setIdentity(); + + MyInternalTriangleIndexCallback cb(colShape, gimpactMesh, depth); + btVector3 aabbMin, aabbMax; + gimpactMesh->getAabb(tr, aabbMin, aabbMax); + gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb, aabbMin, aabbMax); + + return colShape; +} + +#endif //BT_COMPOUND_FROM_GIMPACT diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp index eed31d839f8..f2e3e18d610 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp @@ -27,54 +27,50 @@ 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 + { + } + + 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 + 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 ) const - { - return ( a < b ); - } +public: + bool operator()(const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b) const + { + return (a < b); + } }; - void btContactArray::merge_contacts( - const btContactArray & contacts, bool normal_contact_average) + const btContactArray& contacts, bool normal_contact_average) { clear(); int i; - if(contacts.size()==0) return; - + if (contacts.size() == 0) return; - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts[0]); return; @@ -86,16 +82,16 @@ void btContactArray::merge_contacts( //fill key contacts - for ( i = 0;i<contacts.size() ;i++ ) + for (i = 0; i < contacts.size(); i++) { - keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),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; + int coincident_count = 0; btVector3 coincident_normals[MAX_COINCIDENT]; unsigned int last_key = keycontacts[0].m_key; @@ -103,56 +99,56 @@ void btContactArray::merge_contacts( push_back(contacts[keycontacts[0].m_value]); - GIM_CONTACT * pcontact = &(*this)[0]; + GIM_CONTACT* pcontact = &(*this)[0]; - for( i=1;i<keycontacts.size();i++) + for (i = 1; i < keycontacts.size(); i++) { - key = keycontacts[i].m_key; - const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; + key = keycontacts[i].m_key; + const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value]; - if(last_key == key)//same points + if (last_key == key) //same points { //merge contact - if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //) { *pcontact = *scontact; - coincident_count = 0; + coincident_count = 0; } - else if(normal_contact_average) + 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++; - } - } + 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 + { //add new contact - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } + 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]; - } + push_back(*scontact); + pcontact = &(*this)[this->size() - 1]; + } last_key = key; } } -void btContactArray::merge_contacts_unique(const btContactArray & contacts) +void btContactArray::merge_contacts_unique(const btContactArray& contacts) { clear(); - if(contacts.size()==0) return; + if (contacts.size() == 0) return; - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts[0]); return; @@ -160,14 +156,14 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts) GIM_CONTACT average_contact = contacts[0]; - for (int i=1;i<contacts.size() ;i++ ) + 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()); + btScalar divide_average = 1.0f / ((btScalar)contacts.size()); average_contact.m_point *= divide_average; @@ -176,6 +172,4 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts) 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 index 0c66f8e106c..4ff09d7cddf 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h @@ -27,88 +27,9 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btAlignedObjectArray.h" #include "btTriangleShapeEx.h" +#include "btContactProcessingStructs.h" - - -/** -Configuration var for applying interpolation of contact normals -*/ -#define NORMAL_CONTACT_AVERAGE 1 - -#define CONTACT_DIFF_EPSILON 0.00001f - -///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint. -///@todo: remove and replace GIM_CONTACT by btManifoldPoint. -class GIM_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: - 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) - { - } - - GIM_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<GIM_CONTACT> +class btContactArray : public btAlignedObjectArray<GIM_CONTACT> { public: btContactArray() @@ -117,29 +38,28 @@ public: } SIMD_FORCE_INLINE void push_contact( - const btVector3 &point,const btVector3 & normal, + const btVector3 &point, const btVector3 &normal, btScalar depth, int feature1, int feature2) { - push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) ); + push_back(GIM_CONTACT(point, normal, depth, feature1, feature2)); } SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT & tricontact, - int feature1,int feature2) + const GIM_TRIANGLE_CONTACT &tricontact, + int feature1, int feature2) { - for(int i = 0;i<tricontact.m_point_count ;i++ ) + 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); + tricontact.m_penetration_depth, feature1, feature2); } } - void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true); + void merge_contacts(const btContactArray &contacts, bool normal_contact_average = true); - void merge_contacts_unique(const btContactArray & contacts); + void merge_contacts_unique(const btContactArray &contacts); }; - -#endif // GIM_CONTACT_H_INCLUDED +#endif // GIM_CONTACT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessingStructs.h b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessingStructs.h new file mode 100644 index 00000000000..bc8a709246f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessingStructs.h @@ -0,0 +1,105 @@ +#ifndef BT_CONTACT_H_STRUCTS_INCLUDED +#define BT_CONTACT_H_STRUCTS_INCLUDED + +/*! \file gim_contact.h +\author Francisco Leon Najera +*/ +/* +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" + +/** +Configuration var for applying interpolation of contact normals +*/ +#define NORMAL_CONTACT_AVERAGE 1 + +#define CONTACT_DIFF_EPSILON 0.00001f + +///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint. +///@todo: remove and replace GIM_CONTACT by btManifoldPoint. +class GIM_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: + 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) + { + } + + GIM_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); + } +}; + +#endif // BT_CONTACT_H_STRUCTS_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp index 863233163a6..bb520e061d0 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp @@ -30,7 +30,6 @@ 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(); @@ -45,7 +44,7 @@ void bt_end_gim02_tree_time() //! Gets the average time in miliseconds of tree collisions float btGImpactBvh::getAverageTreeCollisionTime() { - if(g_count_traversing == 0) return 0; + if (g_count_traversing == 0) return 0; float avgtime = g_accum_tree_collision_time; avgtime /= (float)g_count_traversing; @@ -54,80 +53,76 @@ float btGImpactBvh::getAverageTreeCollisionTime() g_count_traversing = 0; return avgtime; -// float avgtime = g_count_traversing; -// g_count_traversing = 0; -// return avgtime; - + // float avgtime = g_count_traversing; + // g_count_traversing = 0; + // return avgtime; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING /////////////////////// btBvhTree ///////////////////////////////// int btBvhTree::_calc_splitting_axis( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_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; + 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++) + 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; + 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); + means *= (btScalar(1.) / (btScalar)numIndices); - for (i=startIndex;i<endIndex;i++) + 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; + 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) ); + variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); return variance.maxAxis(); } - int btBvhTree::_sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; - int splitIndex =startIndex; + 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 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; + 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); + 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++) + 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 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); + primitive_boxes.swap(i, splitIndex); //swapLeafNodes(i,splitIndex); splitIndex++; } @@ -142,32 +137,30 @@ int btBvhTree::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; - } - -void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if ((endIndex-startIndex)==1) + if ((endIndex - startIndex) == 1) { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + //We have a leaf node + setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); return; @@ -175,47 +168,42 @@ void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startI //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); + int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - + 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++) + for (int i = startIndex; i < endIndex; i++) { node_bound.merge(primitive_boxes[i].m_bound); } - setNodeBound(curIndex,node_bound); - + setNodeBound(curIndex, node_bound); //build left branch - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); - + _build_sub_tree(primitive_boxes, startIndex, splitIndex); //build right branch - _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + _build_sub_tree(primitive_boxes, splitIndex, endIndex); m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); - - } //! stackless build tree void btBvhTree::build_tree( - GIM_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY& primitive_boxes) { // initialize node count to 0 m_num_nodes = 0; // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); + m_node_array.resize(primitive_boxes.size() * 2); _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } @@ -225,13 +213,13 @@ void btBvhTree::build_tree( void btGImpactBvh::refit() { int nodecount = getNodeCount(); - while(nodecount--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { btAABB leafbox; - m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { @@ -243,20 +231,20 @@ void btGImpactBvh::refit() btAABB temp_box; int child_node = getLeftNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } child_node = getRightNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } @@ -268,17 +256,17 @@ void btGImpactBvh::buildSet() GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - for (int i = 0;i<primitive_boxes.size() ;i++ ) + 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_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 +bool btGImpactBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -286,7 +274,7 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data @@ -306,19 +294,17 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + 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 + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -326,16 +312,16 @@ bool btGImpactBvh::rayQuery( while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -346,153 +332,133 @@ bool btGImpactBvh::rayQuery( else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + 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) + 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); + 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); + 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, + 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( _node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) + if (boxset0->isLeafNode(node0)) { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { // collision result collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + 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); + 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); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getRightNode(node1), false); } } else { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { - //collide left recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - + 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); - - + 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); + 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); - + 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); + 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); + 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 + } // 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) +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; + if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return; BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - trans_cache_1to0.calc_from_homogenic(trans0,trans1); + trans_cache_1to0.calc_from_homogenic(trans0, trans1); #ifdef TRI_COLLISION_PROFILING bt_begin_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING _find_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); + boxset0, boxset1, + &collision_pairs, trans_cache_1to0, 0, 0, true); #ifdef TRI_COLLISION_PROFILING bt_end_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING } - diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h index 6174ae97a97..3cd8fa24e77 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h @@ -1,5 +1,5 @@ -#ifndef GIM_BOX_SET_H_INCLUDED -#define GIM_BOX_SET_H_INCLUDED +#ifndef BT_GIMPACT_BVH_H_INCLUDED +#define BT_GIMPACT_BVH_H_INCLUDED /*! \file gim_box_set.h \author Francisco Leon Najera @@ -24,135 +24,55 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btAlignedObjectArray.h" #include "btBoxCollision.h" #include "btTriangleShapeEx.h" - - - - - -//! Overlapping pair -struct GIM_PAIR -{ - int m_index1; - int m_index2; - GIM_PAIR() - {} - - GIM_PAIR(const GIM_PAIR & p) - { - m_index1 = p.m_index1; - m_index2 = p.m_index2; - } - - GIM_PAIR(int index1, int index2) - { - m_index1 = index1; - m_index2 = index2; - } -}; +#include "btGImpactBvhStructs.h" //! A pairset array -class btPairSet: public btAlignedObjectArray<GIM_PAIR> +class btPairSet : public btAlignedObjectArray<GIM_PAIR> { public: btPairSet() { reserve(32); } - inline void push_pair(int index1,int index2) + inline void push_pair(int index1, int index2) { - push_back(GIM_PAIR(index1,index2)); + push_back(GIM_PAIR(index1, index2)); } - inline void push_pair_inv(int index1,int index2) + inline void push_pair_inv(int index1, int index2) { - push_back(GIM_PAIR(index2,index1)); + push_back(GIM_PAIR(index2, index1)); } }; - -///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box -struct GIM_BVH_DATA +class GIM_BVH_DATA_ARRAY : public btAlignedObjectArray<GIM_BVH_DATA> { - btAABB m_bound; - int m_data; }; -//! Node Structure for trees -class GIM_BVH_TREE_NODE -{ -public: - btAABB m_bound; -protected: - int m_escapeIndexOrDataIndex; -public: - GIM_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 GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA> -{ -}; - - -class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<GIM_BVH_TREE_NODE> +class GIM_BVH_TREE_NODE_ARRAY : public btAlignedObjectArray<GIM_BVH_TREE_NODE> { }; - - - //! Basic Box tree structure class btBvhTree { protected: int m_num_nodes; GIM_BVH_TREE_NODE_ARRAY m_node_array; + protected: int _sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, - int startIndex, int endIndex, int splitAxis); + GIM_BVH_DATA_ARRAY& primitive_boxes, + int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btBvhTree() { @@ -161,7 +81,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -186,25 +106,25 @@ public: return m_node_array[nodeindex].getDataIndex(); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + 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) + 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; + 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(); + 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 @@ -212,7 +132,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const { return &m_node_array[index]; } @@ -220,7 +140,6 @@ public: //!@} }; - //! Prototype Base class for primitive classification /*! This class is a wrapper for primitive collections. @@ -230,18 +149,16 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime class btPrimitiveManagerBase { public: - virtual ~btPrimitiveManagerBase() {} //! 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; + 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; + 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. @@ -251,13 +168,13 @@ class btGImpactBvh { protected: btBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; + btPrimitiveManagerBase* m_primitive_manager; protected: //stackless refit void refit(); -public: +public: //! this constructor doesn't build the tree. you must call buildSet btGImpactBvh() { @@ -265,31 +182,30 @@ public: } //! this constructor doesn't build the tree. you must call buildSet - btGImpactBvh(btPrimitiveManagerBase * primitive_manager) + btGImpactBvh(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btAABB getGlobalBox() const + SIMD_FORCE_INLINE btAABB getGlobalBox() const { btAABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const { return m_primitive_manager; } - -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -301,21 +217,21 @@ public: void buildSet(); //! returns the indices of the primitives in the m_primitive_manager - bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const; + 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 + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box, + const btTransform& transform, btAlignedObjectArray<int>& collided_results) const { - btAABB transbox=box; + btAABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + 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; + 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 @@ -324,7 +240,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager->is_trimesh(); } @@ -346,17 +262,16 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + 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) + 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); @@ -372,25 +287,23 @@ public: return m_box_tree.getEscapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle); } - - SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const { return m_box_tree.get_node_pointer(index); } #ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING - static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1, - btGImpactBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); + static void find_collision(btGImpactBvh* boxset1, const btTransform& trans1, + btGImpactBvh* boxset2, const btTransform& trans2, + btPairSet& collision_pairs); }; - -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // BT_GIMPACT_BVH_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvhStructs.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvhStructs.h new file mode 100644 index 00000000000..8f78c234b4d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvhStructs.h @@ -0,0 +1,85 @@ +#ifndef GIM_BOX_SET_STRUCT_H_INCLUDED +#define GIM_BOX_SET_STRUCT_H_INCLUDED + +/*! \file gim_box_set.h +\author Francisco Leon Najera +*/ +/* +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" +#include "gim_pair.h" //for GIM_PAIR + +///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box +struct GIM_BVH_DATA +{ + btAABB m_bound; + int m_data; +}; + +//! Node Structure for trees +class GIM_BVH_TREE_NODE +{ +public: + btAABB m_bound; + +protected: + int m_escapeIndexOrDataIndex; + +public: + GIM_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; + } +}; + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp index 2e87475e393..73e3db10107 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ /* -Author: Francisco Len Nßjera +Author: Francisco Leon Najera Concave-Concave Collision */ @@ -31,18 +31,16 @@ Concave-Concave Collision #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) + : btStaticPlaneShape(v, f) { } - void get_plane_equation(btVector4 &equation) + void get_plane_equation(btVector4& equation) { equation[0] = m_planeNormal[0]; equation[1] = m_planeNormal[1]; @@ -50,18 +48,16 @@ public: equation[3] = m_planeConstant; } - - void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const + void get_plane_equation_transformed(const btTransform& trans, btVector4& equation) const { - 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; + const btVector3 normal = trans.getBasis() * m_planeNormal; + equation[0] = normal[0]; + equation[1] = normal[1]; + equation[2] = normal[2]; + equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal)); } }; - - ////////////////////////////////////////////////////////////////////////////////////////////// #ifdef TRI_COLLISION_PROFILING @@ -80,7 +76,7 @@ void bt_end_gim02_tri_time() g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); g_count_triangle_collision++; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING //! Retrieving shapes shapes /*! Declared here due of insuficent space on Pool allocators @@ -89,7 +85,7 @@ Declared here due of insuficent space on Pool allocators class GIM_ShapeRetriever { public: - const btGImpactShapeInterface * m_gim_shape; + const btGImpactShapeInterface* m_gim_shape; btTriangleShapeEx m_trishape; btTetrahedronShapeEx m_tetrashape; @@ -97,51 +93,50 @@ public: class ChildShapeRetriever { public: - GIM_ShapeRetriever * m_parent; - virtual const btCollisionShape * getChildShape(int index) + GIM_ShapeRetriever* m_parent; + virtual const btCollisionShape* getChildShape(int index) { return m_parent->m_gim_shape->getChildShape(index); } virtual ~ChildShapeRetriever() {} }; - class TriangleShapeRetriever:public ChildShapeRetriever + class TriangleShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); + m_parent->m_gim_shape->getBulletTriangle(index, m_parent->m_trishape); return &m_parent->m_trishape; } virtual ~TriangleShapeRetriever() {} }; - class TetraShapeRetriever:public ChildShapeRetriever + class TetraShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape); + 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; + TetraShapeRetriever m_tetra_retriever; + ChildShapeRetriever* m_current_retriever; - GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape) + GIM_ShapeRetriever(const btGImpactShapeInterface* gim_shape) { m_gim_shape = gim_shape; //select retriever - if(m_gim_shape->needsRetrieveTriangles()) + if (m_gim_shape->needsRetrieveTriangles()) { m_current_retriever = &m_tri_retriever; } - else if(m_gim_shape->needsRetrieveTetrahedrons()) + else if (m_gim_shape->needsRetrieveTetrahedrons()) { m_current_retriever = &m_tetra_retriever; } @@ -153,32 +148,26 @@ public: m_current_retriever->m_parent = this; } - const btCollisionShape * getChildShape(int index) + const 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; + if (g_count_triangle_collision == 0) return 0; float avgtime = g_accum_triangle_collision_time; avgtime /= (float)g_count_triangle_collision; @@ -189,12 +178,10 @@ float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() return avgtime; } -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING - -btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap) +btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap) { m_manifoldPtr = NULL; m_convex_algorithm = NULL; @@ -205,77 +192,62 @@ btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() clearCache(); } - - - - -void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btVector3 & point, - const btVector3 & normal, - btScalar distance) +void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btVector3& point, + const btVector3& normal, + btScalar distance) { - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - checkManifold(body0Wrap,body1Wrap); - m_resultOut->addContactPoint(normal,point,distance); + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + checkManifold(body0Wrap, body1Wrap); + m_resultOut->addContactPoint(normal, point, distance); } - void btGImpactCollisionAlgorithm::shape_vs_shape_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape * shape0, - const btCollisionShape * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { - - { - - btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap); + btCollisionAlgorithm* algor = newAlgorithm(body0Wrap, body1Wrap); // post : checkManifold is called - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut); - + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + + algor->processCollision(body0Wrap, body1Wrap, *m_dispatchInfo, m_resultOut); + algor->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algor); } - } void btGImpactCollisionAlgorithm::convex_vs_convex_collision( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape* shape0, - const btCollisionShape* shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1); - checkConvexAlgorithm(&ob0,&ob1); - m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut); - - + btCollisionObjectWrapper ob0(body0Wrap, shape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, shape1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), m_part1, m_triface1); + checkConvexAlgorithm(&ob0, &ob1); + m_convex_algorithm->processCollision(&ob0, &ob1, *m_dispatchInfo, m_resultOut); } - - - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1,btPairSet & pairset) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1, btPairSet& pairset) { - if(shape0->hasBoxSet() && shape1->hasBoxSet()) + if (shape0->hasBoxSet() && shape1->hasBoxSet()) { - btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); + btGImpactBoxSet::find_collision(shape0->getBoxSet(), trans0, shape1->getBoxSet(), trans1, pairset); } else { @@ -283,74 +255,66 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( btAABB boxshape1; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); int j = shape1->getNumChildShapes(); - while(j--) + while (j--) { - shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); + shape1->getChildAabb(i, trans1, boxshape1.m_min, boxshape1.m_max); - if(boxshape1.has_collision(boxshape0)) + if (boxshape1.has_collision(boxshape0)) { - pairset.push_pair(i,j); + pairset.push_pair(i, j); } } } } - - } - void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray<int> & collided_primitives) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, + btAlignedObjectArray<int>& collided_primitives) { - btAABB boxshape; - - if(shape0->hasBoxSet()) + if (shape0->hasBoxSet()) { btTransform trans1to0 = trans0.inverse(); trans1to0 *= trans1; - shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); + 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); + shape1->getAabb(trans1, boxshape.m_min, boxshape.m_max); btAABB boxshape0; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); - if(boxshape.has_collision(boxshape0)) + if (boxshape.has_collision(boxshape0)) { collided_primitives.push_back(i); } } - } - } - -void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) +void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTriangleShapeEx tri0; btTriangleShapeEx tri1; @@ -358,27 +322,22 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + 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); + 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)) + if (tri0.overlap_test_conservative(tri1)) { - convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1); + convex_vs_convex_collision(body0Wrap, body1Wrap, &tri0, &tri1); } - } shape0->unlockChildShapes(); @@ -386,10 +345,10 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW } void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); @@ -401,119 +360,105 @@ void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + while (pair_count--) { - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; - + m_triface1 = *(pair_pointer + 1); + pair_pointer += 2; - shape0->getPrimitiveTriangle(m_triface0,ptri0); - shape1->getPrimitiveTriangle(m_triface1,ptri1); + shape0->getPrimitiveTriangle(m_triface0, ptri0); + shape1->getPrimitiveTriangle(m_triface1, ptri1); - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_begin_gim02_tri_time(); - #endif +#endif ptri0.applyTransform(orgtrans0); ptri1.applyTransform(orgtrans1); - //build planes ptri0.buildTriPlane(); ptri1.buildTriPlane(); // test conservative - - - if(ptri0.overlap_test_conservative(ptri1)) + if (ptri0.overlap_test_conservative(ptri1)) { - if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) + if (ptri0.find_triangle_collision_clip_method(ptri1, contact_data)) { - int j = contact_data.m_point_count; - while(j--) + while (j--) { - addContactPoint(body0Wrap, body1Wrap, - contact_data.m_points[j], - contact_data.m_separating_normal, - -contact_data.m_penetration_depth); + contact_data.m_points[j], + contact_data.m_separating_normal, + -contact_data.m_penetration_depth); } } } - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_end_gim02_tri_time(); - #endif - +#endif } shape0->unlockChildShapes(); shape1->unlockChildShapes(); - } - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1) { - - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); + const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0); m_part0 = meshshape0->getMeshPartCount(); - while(m_part0--) + while (m_part0--) { - gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, meshshape0->getMeshPart(m_part0), shape1); } return; } - if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1); + const btGImpactMeshShape* meshshape1 = static_cast<const btGImpactMeshShape*>(shape1); m_part1 = meshshape1->getMeshPartCount(); - while(m_part1--) + while (m_part1--) { - - gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1)); - + gimpact_vs_gimpact(body0Wrap, body1Wrap, shape0, meshshape1->getMeshPart(m_part1)); } return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btPairSet pairset; - gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); + gimpact_vs_gimpact_find_pairs(orgtrans0, orgtrans1, shape0, shape1, pairset); - if(pairset.size()== 0) return; + if (pairset.size() == 0) return; - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) { - const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0); - const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1); - //specialized function - #ifdef BULLET_TRIANGLE_COLLISION - collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #else - collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #endif + const btGImpactMeshShapePart* shapepart0 = static_cast<const btGImpactMeshShapePart*>(shape0); + const btGImpactMeshShapePart* shapepart1 = static_cast<const btGImpactMeshShapePart*>(shape1); +//specialized function +#ifdef BULLET_TRIANGLE_COLLISION + collide_gjk_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#else + collide_sat_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#endif return; } @@ -530,32 +475,32 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( bool child_has_transform1 = shape1->childrenHasTransform(); int i = pairset.size(); - while(i--) + while (i--) { - GIM_PAIR * pair = &pairset[i]; + GIM_PAIR* pair = &pairset[i]; m_triface0 = pair->m_index1; m_triface1 = pair->m_index2; - const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); - const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); + const btCollisionShape* colshape0 = retriever0.getChildShape(m_triface0); + const btCollisionShape* colshape1 = retriever1.getChildShape(m_triface1); btTransform tr0 = body0Wrap->getWorldTransform(); btTransform tr1 = body1Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(m_triface0); + tr0 = orgtrans0 * shape0->getChildTransform(m_triface0); } - if(child_has_transform1) + if (child_has_transform1) { - tr1 = orgtrans1*shape1->getChildTransform(m_triface1); + tr1 = orgtrans1 * shape1->getChildTransform(m_triface1); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1); + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), tr0, m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), tr1, m_part1, m_triface1); //collide two convex shapes - convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1); + convex_vs_convex_collision(&ob0, &ob1, colshape0, colshape1); } shape0->unlockChildShapes(); @@ -563,159 +508,159 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( } void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, bool swapped) { - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); + const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0); int& part = swapped ? m_part1 : m_part0; part = meshshape0->getMeshPartCount(); - while(part--) + while (part--) { - gimpact_vs_shape(body0Wrap, - body1Wrap, - meshshape0->getMeshPart(part), - shape1,swapped); - + body1Wrap, + meshshape0->getMeshPart(part), + shape1, swapped); } return; } - #ifdef GIMPACT_VS_PLANE_COLLISION - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && +#ifdef GIMPACT_VS_PLANE_COLLISION + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) { - const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0); - const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1); - gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped); + const btGImpactMeshShapePart* shapepart = static_cast<const btGImpactMeshShapePart*>(shape0); + const btStaticPlaneShape* planeshape = static_cast<const btStaticPlaneShape*>(shape1); + gimpacttrimeshpart_vs_plane_collision(body0Wrap, body1Wrap, shapepart, planeshape, swapped); return; } - #endif - +#endif - - if(shape1->isCompound()) + if (shape1->isCompound()) { - const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1); - gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped); + const btCompoundShape* compoundshape = static_cast<const btCompoundShape*>(shape1); + gimpact_vs_compoundshape(body0Wrap, body1Wrap, shape0, compoundshape, swapped); return; } - else if(shape1->isConcave()) + else if (shape1->isConcave()) { - const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1); - gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped); + const btConcaveShape* concaveshape = static_cast<const btConcaveShape*>(shape1); + gimpact_vs_concave(body0Wrap, body1Wrap, shape0, concaveshape, swapped); return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btAlignedObjectArray<int> collided_results; - gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results); - - if(collided_results.size() == 0) return; + 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--) + while (i--) { int child_index = collided_results[i]; - if(swapped) - m_triface1 = child_index; - else - m_triface0 = child_index; + if (swapped) + m_triface1 = child_index; + else + m_triface0 = child_index; - const btCollisionShape * colshape0 = retriever0.getChildShape(child_index); + const btCollisionShape* colshape0 = retriever0.getChildShape(child_index); btTransform tr0 = body0Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(child_index); + tr0 = orgtrans0 * shape0->getChildTransform(child_index); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); - const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap(); - - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject()) + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); + const btCollisionObjectWrapper* prevObj; + + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject()) { + prevObj = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&ob0); - } else + } + else { + prevObj = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&ob0); } //collide two shapes - if(swapped) + if (swapped) { - - shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0); + shape_vs_shape_collision(body1Wrap, &ob0, shape1, colshape0); } else { - - shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1); + shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1); } - m_resultOut->setBody0Wrap(prevObj0); + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject()) + { + m_resultOut->setBody0Wrap(prevObj); + } + else + { + m_resultOut->setBody1Wrap(prevObj); + } } shape0->unlockChildShapes(); - } void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCompoundShape* shape1, bool swapped) { btTransform orgtrans1 = body1Wrap->getWorldTransform(); int i = shape1->getNumChildShapes(); - while(i--) + while (i--) { + const btCollisionShape* colshape1 = shape1->getChildShape(i); + btTransform childtrans1 = orgtrans1 * shape1->getChildTransform(i); - const btCollisionShape * colshape1 = shape1->getChildShape(i); - btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), childtrans1, -1, i); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i); - const btCollisionObjectWrapper* tmp = 0; - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { tmp = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&ob1); - } else + } + else { tmp = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&ob1); } //collide child shape gimpact_vs_shape(body0Wrap, &ob1, - shape0,colshape1,swapped); + shape0, colshape1, swapped); - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { m_resultOut->setBody0Wrap(tmp); - } else + } + else { m_resultOut->setBody1Wrap(tmp); } @@ -723,27 +668,25 @@ void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObje } void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btStaticPlaneShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btStaticPlaneShape* shape1, bool swapped) { - - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); - const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1); + const btPlaneShape* planeshape = static_cast<const btPlaneShape*>(shape1); btVector4 plane; - planeshape->get_plane_equation_transformed(orgtrans1,plane); + planeshape->get_plane_equation_transformed(orgtrans1, plane); //test box against plane btAABB tribox; - shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); + shape0->getAabb(orgtrans0, tribox.m_min, tribox.m_max); tribox.increment_margin(planeshape->getMargin()); - if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; + if (tribox.plane_classify(plane) != BT_CONST_COLLIDE_PLANE) return; shape0->lockChildShapes(); @@ -751,28 +694,28 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( btVector3 vertex; int vi = shape0->getVertexCount(); - while(vi--) + while (vi--) { - shape0->getVertex(vi,vertex); + shape0->getVertex(vi, vertex); vertex = orgtrans0(vertex); btScalar distance = vertex.dot(plane) - plane[3] - margin; - if(distance<0.0)//add contact + if (distance < 0.0) //add contact { - if(swapped) + if (swapped) { addContactPoint(body1Wrap, body0Wrap, - vertex, - -plane, - distance); + vertex, + -plane, + distance); } else { addContactPoint(body0Wrap, body1Wrap, - vertex, - plane, - distance); + vertex, + plane, + distance); } } } @@ -780,69 +723,64 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( shape0->unlockChildShapes(); } - - - -class btGImpactTriangleCallback: public btTriangleCallback +class btGImpactTriangleCallback : public btTriangleCallback { public: - btGImpactCollisionAlgorithm * algorithm; - const btCollisionObjectWrapper * body0Wrap; - const btCollisionObjectWrapper * body1Wrap; - const btGImpactShapeInterface * gimpactshape0; + btGImpactCollisionAlgorithm* algorithm; + const btCollisionObjectWrapper* body0Wrap; + const btCollisionObjectWrapper* body1Wrap; + const btGImpactShapeInterface* gimpactshape0; bool swapped; btScalar margin; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); + 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); - } - - btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex); - const btCollisionObjectWrapper * tmp = 0; - - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + if (swapped) + { + algorithm->setPart0(partId); + algorithm->setFace0(triangleIndex); + } + else + { + algorithm->setPart1(partId); + algorithm->setFace1(triangleIndex); + } + + btCollisionObjectWrapper ob1Wrap(body1Wrap, &tri1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), partId, triangleIndex); + const btCollisionObjectWrapper* tmp = 0; + + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { tmp = algorithm->internalGetResultOut()->getBody0Wrap(); algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap); - } else + } + else { tmp = algorithm->internalGetResultOut()->getBody1Wrap(); algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap); } - + algorithm->gimpact_vs_shape( - body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped); + body0Wrap, &ob1Wrap, gimpactshape0, &tri1, swapped); - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { algorithm->internalGetResultOut()->setBody0Wrap(tmp); - } else + } + else { algorithm->internalGetResultOut()->setBody1Wrap(tmp); } - } }; - - - void btGImpactCollisionAlgorithm::gimpact_vs_concave( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btConcaveShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btConcaveShape* shape1, bool swapped) { //create the callback btGImpactTriangleCallback tricallback; @@ -858,75 +796,71 @@ void btGImpactCollisionAlgorithm::gimpact_vs_concave( gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform(); - btVector3 minAABB,maxAABB; - shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); - - shape1->processAllTriangles(&tricallback,minAABB,maxAABB); + btVector3 minAABB, maxAABB; + shape0->getAabb(gimpactInConcaveSpace, minAABB, maxAABB); + shape1->processAllTriangles(&tricallback, minAABB, maxAABB); } - - -void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btGImpactCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - clearCache(); + clearCache(); - m_resultOut = resultOut; + m_resultOut = resultOut; m_dispatchInfo = &dispatchInfo; - const btGImpactShapeInterface * gimpactshape0; - const btGImpactShapeInterface * gimpactshape1; + const btGImpactShapeInterface* gimpactshape0; + const btGImpactShapeInterface* gimpactshape1; - if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) + if (body0Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape()); + gimpactshape0 = static_cast<const btGImpactShapeInterface*>(body0Wrap->getCollisionShape()); - if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape()); - gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, gimpactshape0, gimpactshape1); } else { - gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false); + gimpact_vs_shape(body0Wrap, body1Wrap, gimpactshape0, body1Wrap->getCollisionShape(), false); } - } - else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + else if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape()); - gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true); + gimpact_vs_shape(body1Wrap, body0Wrap, gimpactshape1, body0Wrap->getCollisionShape(), true); } -} + // Ensure that gContactProcessedCallback is called for concave shapes. + if (getLastManifold()) + { + m_resultOut->refreshContactPoints(); + } +} -btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { return 1.f; - } ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// - - //! Use this function for register the algorithm externally -void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) +void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher* dispatcher) { - static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf; int i; - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE, i, &s_gimpact_cf); } - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(i, GIMPACT_SHAPE_PROXYTYPE, &s_gimpact_cf); } - } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index f85a94cb4c7..a368c8a0c0b 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -42,7 +42,6 @@ class btDispatcher; #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - //! Collision Algorithm for GImpact Shapes /*! For register this algorithm in Bullet, proceed as following: @@ -54,36 +53,35 @@ btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm { protected: - btCollisionAlgorithm * m_convex_algorithm; - btPersistentManifold * m_manifoldPtr; + btCollisionAlgorithm* m_convex_algorithm; + btPersistentManifold* m_manifoldPtr; btManifoldResult* m_resultOut; - const btDispatcherInfo * m_dispatchInfo; + 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(const btCollisionObject* body0,const btCollisionObject* body1) + SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0, const btCollisionObject* body1) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_manifoldPtr = m_dispatcher->getNewManifold(body0, body1); return m_manifoldPtr; } SIMD_FORCE_INLINE void destroyConvexAlgorithm() { - if(m_convex_algorithm) + if (m_convex_algorithm) { m_convex_algorithm->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm); + m_dispatcher->freeCollisionAlgorithm(m_convex_algorithm); m_convex_algorithm = NULL; } } SIMD_FORCE_INLINE void destroyContactManifolds() { - if(m_manifoldPtr == NULL) return; + if (m_manifoldPtr == NULL) return; m_dispatcher->releaseManifold(m_manifoldPtr); m_manifoldPtr = NULL; } @@ -104,207 +102,187 @@ protected: return m_manifoldPtr; } - // Call before process collision - SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - if(getLastManifold() == 0) + if (getLastManifold() == 0) { - newContactManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + newContactManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); } m_resultOut->setPersistentManifold(getLastManifold()); } // Call before process collision - SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE btCollisionAlgorithm* newAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - checkManifold(body0Wrap,body1Wrap); + checkManifold(body0Wrap, body1Wrap); - btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( - body0Wrap,body1Wrap,getLastManifold()); - return convex_algorithm ; + btCollisionAlgorithm* convex_algorithm = m_dispatcher->findAlgorithm( + body0Wrap, body1Wrap, getLastManifold(), BT_CONTACT_POINT_ALGORITHMS); + return convex_algorithm; } // Call before process collision - SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - if(m_convex_algorithm) return; - m_convex_algorithm = newAlgorithm(body0Wrap,body1Wrap); + if (m_convex_algorithm) return; + m_convex_algorithm = newAlgorithm(body0Wrap, body1Wrap); } + void addContactPoint(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btVector3& point, + const btVector3& normal, + btScalar distance); - - - void addContactPoint(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btVector3 & point, - const btVector3 & normal, - btScalar distance); - -//! Collision routines -//!@{ + //! Collision routines + //!@{ void collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count); void collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count); - - - + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count); void shape_vs_shape_collision( - const btCollisionObjectWrapper* body0, - const btCollisionObjectWrapper* body1, - const btCollisionShape * shape0, - const btCollisionShape * shape1); + const btCollisionObjectWrapper* body0, + const btCollisionObjectWrapper* body1, + const btCollisionShape* shape0, + const btCollisionShape* shape1); void convex_vs_convex_collision(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape* shape0, - const btCollisionShape* shape1); - - + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1); void gimpact_vs_gimpact_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1,btPairSet & pairset); + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1, btPairSet& pairset); void gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray<int> & collided_primitives); - + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, + btAlignedObjectArray<int>& collided_primitives); void gimpacttrimeshpart_vs_plane_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btStaticPlaneShape * shape1,bool swapped); - + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btStaticPlaneShape* shape1, bool swapped); public: - - btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btGImpactCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr) manifoldArray.push_back(m_manifoldPtr); } - btManifoldResult* internalGetResultOut() + btManifoldResult* internalGetResultOut() { return m_resultOut; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm)); - return new(mem) btGImpactCollisionAlgorithm(ci,body0Wrap,body1Wrap); + return new (mem) btGImpactCollisionAlgorithm(ci, body0Wrap, body1Wrap); } }; //! Use this function for register the algorithm externally - static void registerAlgorithm(btCollisionDispatcher * dispatcher); + static void registerAlgorithm(btCollisionDispatcher* dispatcher); #ifdef TRI_COLLISION_PROFILING //! Gets the average time in miliseconds of tree collisions static float getAverageTreeCollisionTime(); //! Gets the average time in miliseconds of triangle collisions static float getAverageTriangleCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING //! Collides two gimpact shapes /*! \pre shape0 and shape1 couldn't be btGImpactMeshShape objects */ - void gimpact_vs_gimpact(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1); void gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, bool swapped); - void gimpact_vs_compoundshape(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped); + void gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCompoundShape* shape1, bool swapped); void gimpact_vs_concave( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const 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; - } + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const 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 //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h index 2543aefcfc6..1cde46ed8ba 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h @@ -21,40 +21,36 @@ subject to the following restrictions: 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) + const btVector3& source_inertia, const btVector3& added_inertia, const btTransform& transform) { - btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); + btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); btScalar x2 = transform.getOrigin()[0]; - x2*= x2; + x2 *= x2; btScalar y2 = transform.getOrigin()[1]; - y2*= y2; + y2 *= y2; btScalar z2 = transform.getOrigin()[2]; - z2*= z2; + z2 *= z2; - btScalar ix = rotatedTensor[0][0]*(y2+z2); - btScalar iy = rotatedTensor[1][1]*(x2+z2); - btScalar iz = rotatedTensor[2][2]*(x2+y2); + 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); + 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) +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)); + 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 +#endif //GIMPACT_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp index 4528758c370..b81fc970449 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -27,11 +27,9 @@ subject to the following restrictions: #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(); @@ -43,11 +41,10 @@ void bt_end_gim02_q_tree_time() g_q_count_traversing++; } - //! Gets the average time in miliseconds of tree collisions float btGImpactQuantizedBvh::getAverageTreeCollisionTime() { - if(g_q_count_traversing == 0) return 0; + if (g_q_count_traversing == 0) return 0; float avgtime = g_q_accum_tree_collision_time; avgtime /= (float)g_q_count_traversing; @@ -56,99 +53,92 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime() g_q_count_traversing = 0; return avgtime; -// float avgtime = g_q_count_traversing; -// g_q_count_traversing = 0; -// return avgtime; - + // float avgtime = g_q_count_traversing; + // g_q_count_traversing = 0; + // return avgtime; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING /////////////////////// btQuantizedBvhTree ///////////////////////////////// void btQuantizedBvhTree::calc_quantization( - GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) + GIM_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++ ) + 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); - + 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( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_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; + 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++) + 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; + 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); + means *= (btScalar(1.) / (btScalar)numIndices); - for (i=startIndex;i<endIndex;i++) + 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; + 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) ); + variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); return variance.maxAxis(); } - int btQuantizedBvhTree::_sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; - int splitIndex =startIndex; + 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 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; + 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); + 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++) + 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 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); + primitive_boxes.swap(i, splitIndex); //swapLeafNodes(i,splitIndex); splitIndex++; } @@ -163,32 +153,30 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; - } - -void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if ((endIndex-startIndex)==1) + if ((endIndex - startIndex) == 1) { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + //We have a leaf node + setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); return; @@ -196,48 +184,43 @@ void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, i //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); + int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - + 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++) + for (int i = startIndex; i < endIndex; i++) { node_bound.merge(primitive_boxes[i].m_bound); } - setNodeBound(curIndex,node_bound); - + setNodeBound(curIndex, node_bound); //build left branch - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); - + _build_sub_tree(primitive_boxes, startIndex, splitIndex); //build right branch - _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + _build_sub_tree(primitive_boxes, splitIndex, endIndex); m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); - - } //! stackless build tree void btQuantizedBvhTree::build_tree( - GIM_BVH_DATA_ARRAY & primitive_boxes) + GIM_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); + m_node_array.resize(primitive_boxes.size() * 2); _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } @@ -247,13 +230,13 @@ void btQuantizedBvhTree::build_tree( void btGImpactQuantizedBvh::refit() { int nodecount = getNodeCount(); - while(nodecount--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { btAABB leafbox; - m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { @@ -265,20 +248,20 @@ void btGImpactQuantizedBvh::refit() btAABB temp_box; int child_node = getLeftNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } child_node = getRightNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } @@ -290,17 +273,17 @@ void btGImpactQuantizedBvh::buildSet() GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - for (int i = 0;i<primitive_boxes.size() ;i++ ) + 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_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 +bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -310,16 +293,14 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in unsigned short quantizedMin[3]; unsigned short quantizedMax[3]; - m_box_tree.quantizePoint(quantizedMin,box.m_min); - m_box_tree.quantizePoint(quantizedMax,box.m_max); - + 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 aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) @@ -335,19 +316,17 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + 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 + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -355,16 +334,16 @@ bool btGImpactQuantizedBvh::rayQuery( while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -375,154 +354,133 @@ bool btGImpactQuantizedBvh::rayQuery( else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - SIMD_FORCE_INLINE bool _quantized_node_collision( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0 ,int node1, bool complete_primitive_tests) + const btGImpactQuantizedBvh* boxset0, const 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); + 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); + 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( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - btPairSet * collision_pairs, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + const btGImpactQuantizedBvh* boxset0, const 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( _quantized_node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) + if (boxset0->isLeafNode(node0)) { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { // collision result collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + 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); + 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); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getRightNode(node1), false); } } else { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { - //collide left recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - + 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); - - + 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); + 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); - + 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); + 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); + 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 + } // else if node1 is not a leaf + } // else if node0 is not a leaf } - -void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0, - const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) +void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0, + const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, + btPairSet& collision_pairs) { - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return; BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - trans_cache_1to0.calc_from_homogenic(trans0,trans1); + trans_cache_1to0.calc_from_homogenic(trans0, trans1); #ifdef TRI_COLLISION_PROFILING bt_begin_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); + boxset0, boxset1, + &collision_pairs, trans_cache_1to0, 0, 0, true); #ifdef TRI_COLLISION_PROFILING bt_end_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING } - - diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h index e6e52fff4c0..b231c1e8327 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -26,81 +26,12 @@ subject to the following restrictions: #include "btGImpactBvh.h" #include "btQuantization.h" +#include "btGImpactQuantizedBvhStructs.h" - - - - -///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 GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE> +class GIM_QUANTIZED_BVH_NODE_ARRAY : public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE> { }; - - - //! Basic Box tree structure class btQuantizedBvhTree { @@ -109,16 +40,18 @@ protected: GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array; btAABB m_global_bound; btVector3 m_bvhQuantization; + protected: - void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); + void calc_quantization(GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin = btScalar(1.0)); int _sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, - int startIndex, int endIndex, int splitAxis); + GIM_BVH_DATA_ARRAY& primitive_boxes, + int startIndex, int endIndex, int splitAxis); + + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); - int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btQuantizedBvhTree() { @@ -127,20 +60,19 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes); SIMD_FORCE_INLINE void quantizePoint( - unsigned short * quantizedpoint, const btVector3 & point) const + unsigned short* quantizedpoint, const btVector3& point) const { - bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization); + 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 + unsigned short* quantizedMin, unsigned short* quantizedMax) const { - return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax); + return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin, quantizedMax); } SIMD_FORCE_INLINE void clearNodes() @@ -166,41 +98,41 @@ public: return m_node_array[nodeindex].getDataIndex(); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + 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); + 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); + m_global_bound.m_min, m_bvhQuantization); } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + 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_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); + 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; + 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(); + 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 @@ -208,7 +140,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const { return &m_node_array[index]; } @@ -216,8 +148,6 @@ public: //!@} }; - - //! Structure for containing Boxes /*! This class offers an structure for managing a box tree of primitives. @@ -227,13 +157,13 @@ class btGImpactQuantizedBvh { protected: btQuantizedBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; + btPrimitiveManagerBase* m_primitive_manager; protected: //stackless refit void refit(); -public: +public: //! this constructor doesn't build the tree. you must call buildSet btGImpactQuantizedBvh() { @@ -241,31 +171,30 @@ public: } //! this constructor doesn't build the tree. you must call buildSet - btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager) + btGImpactQuantizedBvh(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btAABB getGlobalBox() const + SIMD_FORCE_INLINE btAABB getGlobalBox() const { btAABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const { return m_primitive_manager; } - -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -277,21 +206,21 @@ public: void buildSet(); //! returns the indices of the primitives in the m_primitive_manager - bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const; + 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 + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box, + const btTransform& transform, btAlignedObjectArray<int>& collided_results) const { - btAABB transbox=box; + btAABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + 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; + 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 @@ -300,7 +229,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager->is_trimesh(); } @@ -322,17 +251,16 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + 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) + 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); @@ -348,25 +276,23 @@ public: return m_box_tree.getEscapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle); } - - SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const { return m_box_tree.get_node_pointer(index); } #ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING - static void find_collision(const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - const btGImpactQuantizedBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); + static void find_collision(const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, + const btGImpactQuantizedBvh* boxset2, const btTransform& trans2, + btPairSet& collision_pairs); }; - -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h new file mode 100644 index 00000000000..bd50cb5b87e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h @@ -0,0 +1,91 @@ +#ifndef GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED +#define GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED + +/*! \file btGImpactQuantizedBvh.h +\author Francisco Leon Najera +*/ +/* +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" + +///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; + } +}; + +#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp index ac8efdf3833..34c229a3aba 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp @@ -18,125 +18,169 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btGImpactShape.h" #include "btGImpactMassUtil.h" +btGImpactMeshShapePart::btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part) +{ + // moved from .h to .cpp because of conditional compilation + // (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to + // avoid using it in h files) + m_primitive_manager.m_meshInterface = meshInterface; + m_primitive_manager.m_part = part; + m_box_set.setPrimitiveManager(&m_primitive_manager); +#if BT_THREADSAFE + // If threadsafe is requested, this object uses a different lock/unlock + // model with the btStridingMeshInterface -- lock once when the object is constructed + // and unlock once in the destructor. + // The other way of locking and unlocking for each collision check in the narrowphase + // is not threadsafe. Note these are not thread-locks, they are calls to the meshInterface's + // getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of + // pointers. In theory a client could override the lock function to do all sorts of + // things like reading data from GPU memory, or decompressing data on the fly, but such things + // do not seem all that likely or useful, given the performance cost. + m_primitive_manager.lock(); +#endif +} -#define CALC_EXACT_INERTIA 1 +btGImpactMeshShapePart::~btGImpactMeshShapePart() +{ + // moved from .h to .cpp because of conditional compilation +#if BT_THREADSAFE + m_primitive_manager.unlock(); +#endif +} +void btGImpactMeshShapePart::lockChildShapes() const +{ + // moved from .h to .cpp because of conditional compilation +#if !BT_THREADSAFE + // called in the narrowphase -- not threadsafe! + void* dummy = (void*)(m_box_set.getPrimitiveManager()); + TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy); + dummymanager->lock(); +#endif +} -void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactMeshShapePart::unlockChildShapes() const +{ + // moved from .h to .cpp because of conditional compilation +#if !BT_THREADSAFE + // called in the narrowphase -- not threadsafe! + void* dummy = (void*)(m_box_set.getPrimitiveManager()); + TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy); + dummymanager->unlock(); +#endif +} + +#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); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getNumChildShapes(); - btScalar shapemass = mass/btScalar(i); + btScalar shapemass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 temp_inertia; - m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia); - if(childrenHasTransform()) + m_childShapes[i]->calculateLocalInertia(shapemass, temp_inertia); + if (childrenHasTransform()) { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]); + inertia = gim_inertia_add_transformed(inertia, temp_inertia, m_childTransforms[i]); } else { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity()); + 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; + 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)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif unlockChildShapes(); } - - -void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass, btVector3& inertia) const { lockChildShapes(); - #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getVertexCount(); - btScalar pointmass = mass/btScalar(i); + btScalar pointmass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 pointintertia; - this->getVertex(i,pointintertia); - pointintertia = gim_get_point_inertia(pointintertia,pointmass); - inertia+=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; + 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)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif unlockChildShapes(); } -void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getMeshPartCount(); - btScalar partmass = mass/btScalar(i); + btScalar partmass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 partinertia; - getMeshPart(i)->calculateLocalInertia(partmass,partinertia); - inertia+=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; + 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)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif } @@ -145,7 +189,7 @@ void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayT { } -void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const +void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const { lockChildShapes(); @@ -154,7 +198,7 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback rayDir.normalize(); m_box_set.rayQuery(rayDir, rayFrom, collided); - if(collided.size()==0) + if (collided.size() == 0) { unlockChildShapes(); return; @@ -163,15 +207,15 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback int part = (int)getPart(); btPrimitiveTriangle triangle; int i = collided.size(); - while(i--) + while (i--) { - getPrimitiveTriangle(collided[i],triangle); - callback->processTriangle(triangle.m_vertices,part,collided[i]); + getPrimitiveTriangle(collided[i], triangle); + callback->processTriangle(triangle.m_vertices, part, collided[i]); } unlockChildShapes(); } -void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { lockChildShapes(); btAABB box; @@ -179,9 +223,9 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co box.m_max = aabbMax; btAlignedObjectArray<int> collided; - m_box_set.boxQuery(box,collided); + m_box_set.boxQuery(box, collided); - if(collided.size()==0) + if (collided.size() == 0) { unlockChildShapes(); return; @@ -190,40 +234,38 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co int part = (int)getPart(); btPrimitiveTriangle triangle; int i = collided.size(); - while(i--) + while (i--) { - this->getPrimitiveTriangle(collided[i],triangle); - callback->processTriangle(triangle.m_vertices,part,collided[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 +void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { int i = m_mesh_parts.size(); - while(i--) + while (i--) { - m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax); + m_mesh_parts[i]->processAllTriangles(callback, aabbMin, aabbMax); } } -void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const +void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const { int i = m_mesh_parts.size(); - while(i--) + while (i--) { m_mesh_parts[i]->processAllTrianglesRay(callback, rayFrom, rayTo); } } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*) dataBuffer; + btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*)dataBuffer; - btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer); m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); @@ -235,4 +277,3 @@ const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serial return "btGImpactMeshShapeData"; } - diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h index 3d1f48d4776..cc91079579d 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h @@ -21,7 +21,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_SHAPE_H #define GIMPACT_SHAPE_H @@ -37,8 +36,7 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAlignedObjectArray.h" -#include "btGImpactQuantizedBvh.h" // box tree class - +#include "btGImpactQuantizedBvh.h" // box tree class //! declare Quantized trees, (you can change to float based trees) typedef btGImpactQuantizedBvh btGImpactBoxSet; @@ -50,10 +48,8 @@ enum eGIMPACT_SHAPE_TYPE CONST_GIMPACT_TRIMESH_SHAPE }; - - //! Helper class for tetrahedrons -class btTetrahedronShapeEx:public btBU_Simplex1to4 +class btTetrahedronShapeEx : public btBU_Simplex1to4 { public: btTetrahedronShapeEx() @@ -61,10 +57,9 @@ public: m_numVertices = 4; } - SIMD_FORCE_INLINE void setVertices( - const btVector3 & v0,const btVector3 & v1, - const btVector3 & v2,const btVector3 & v3) + const btVector3& v0, const btVector3& v1, + const btVector3& v2, const btVector3& v3) { m_vertices[0] = v0; m_vertices[1] = v1; @@ -74,45 +69,42 @@ public: } }; - //! 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 + 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() - { + //! 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(); - } + 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_shapeType=GIMPACT_SHAPE_PROXYTYPE; + m_shapeType = GIMPACT_SHAPE_PROXYTYPE; m_localAABB.invalidate(); m_needs_update = true; - localScaling.setValue(1.f,1.f,1.f); + localScaling.setValue(1.f, 1.f, 1.f); } - //! performs refit operation /*! Updates the entire Box set of this shape. @@ -120,47 +112,46 @@ public: 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. - /*! + 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; - } + 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() + SIMD_FORCE_INLINE const btAABB& getLocalBox() { return m_localAABB; } + virtual int getShapeType() const + { + return GIMPACT_SHAPE_PROXYTYPE; + } - virtual int getShapeType() const - { - return GIMPACT_SHAPE_PROXYTYPE; - } - - /*! + /*! \post You must call updateBound() for update the box set. */ - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { localScaling = scaling; postUpdate(); @@ -171,46 +162,43 @@ public: return localScaling; } - virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; - int i = getNumChildShapes(); - while(i--) - { + { + 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() const = 0 ; + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0; //! gets boxset - SIMD_FORCE_INLINE const btGImpactBoxSet * getBoxSet() const + SIMD_FORCE_INLINE const btGImpactBoxSet* getBoxSet() const { return &m_box_set; } //! Determines if this class has a hierarchy structure for sorting its primitives - SIMD_FORCE_INLINE bool hasBoxSet() const + SIMD_FORCE_INLINE bool hasBoxSet() const { - if(m_box_set.getNodeCount() == 0) return false; + if (m_box_set.getNodeCount() == 0) return false; return true; } //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const = 0; - + virtual const btPrimitiveManagerBase* getPrimitiveManager() const = 0; //! Gets the number of children - virtual int getNumChildShapes() const = 0; + virtual int getNumChildShapes() const = 0; //! if true, then its children must get transforms. virtual bool childrenHasTransform() const = 0; @@ -221,11 +209,9 @@ public: //! 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; - + 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 @@ -237,94 +223,91 @@ public: } //! if this trimesh - SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getPrimitiveTriangle(int index, btPrimitiveTriangle& triangle) const { - getPrimitiveManager()->get_primitive_triangle(index,triangle); + 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; - } + 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; + 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 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 + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const { - (void) rayFrom; (void) rayTo; (void) resultCallback; + (void)rayFrom; + (void)rayTo; + (void)resultCallback; } //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - (void) callback; (void) aabbMin; (void) aabbMax; + (void)callback; + (void)aabbMin; + (void)aabbMax; } //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/,const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const + virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/, const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const { - } //!@} - }; - //! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once /*! This class only can manage Convex subshapes */ -class btGImpactCompoundShape : public btGImpactShapeInterface +class btGImpactCompoundShape : public btGImpactShapeInterface { public: //! compound primitive manager - class CompoundPrimitiveManager:public btPrimitiveManagerBase + class CompoundPrimitiveManager : public btPrimitiveManagerBase { public: virtual ~CompoundPrimitiveManager() {} - btGImpactCompoundShape * m_compoundShape; - + btGImpactCompoundShape* m_compoundShape; CompoundPrimitiveManager(const CompoundPrimitiveManager& compound) - : btPrimitiveManagerBase() + : btPrimitiveManagerBase() { m_compoundShape = compound.m_compoundShape; } - CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape) + CompoundPrimitiveManager(btGImpactCompoundShape* compoundShape) { m_compoundShape = compoundShape; } @@ -341,13 +324,13 @@ public: virtual int get_primitive_count() const { - return (int )m_compoundShape->getNumChildShapes(); + return (int)m_compoundShape->getNumChildShapes(); } - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const + virtual void get_primitive_box(int prim_index, btAABB& primbox) const { btTransform prim_trans; - if(m_compoundShape->childrenHasTransform()) + if (m_compoundShape->childrenHasTransform()) { prim_trans = m_compoundShape->getChildTransform(prim_index); } @@ -356,30 +339,26 @@ public: prim_trans.setIdentity(); } const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index); - shape->getAabb(prim_trans,primbox.m_min,primbox.m_max); + shape->getAabb(prim_trans, primbox.m_min, primbox.m_max); } - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const { btAssert(0); - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; } - }; - - protected: CompoundPrimitiveManager m_primitive_manager; - btAlignedObjectArray<btTransform> m_childTransforms; - btAlignedObjectArray<btCollisionShape*> m_childShapes; - + btAlignedObjectArray<btTransform> m_childTransforms; + btAlignedObjectArray<btCollisionShape*> m_childShapes; public: - btGImpactCompoundShape(bool children_has_transform = true) { - (void) children_has_transform; + (void)children_has_transform; m_primitive_manager.m_compoundShape = this; m_box_set.setPrimitiveManager(&m_primitive_manager); } @@ -388,36 +367,33 @@ public: { } - //! if true, then its children must get transforms. virtual bool childrenHasTransform() const { - if(m_childTransforms.size()==0) return false; + if (m_childTransforms.size() == 0) return false; return true; } - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { return &m_primitive_manager; } //! Obtains the compopund primitive manager - SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager() + SIMD_FORCE_INLINE CompoundPrimitiveManager* getCompoundPrimitiveManager() { return &m_primitive_manager; } //! Gets the number of children - virtual int getNumChildShapes() const + 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) + void addChildShape(const btTransform& localTransform, btCollisionShape* shape) { btAssert(shape->isConvex()); m_childTransforms.push_back(localTransform); @@ -444,24 +420,22 @@ public: } //! 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); - } - } - + 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 + virtual btTransform getChildTransform(int index) const { btAssert(m_childTransforms.size() == m_childShapes.size()); return m_childTransforms[index]; @@ -471,7 +445,7 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { btAssert(m_childTransforms.size() == m_childShapes.size()); m_childTransforms[index] = transform; @@ -490,24 +464,24 @@ public: return false; } - - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; btAssert(0); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } - //! Calculates the exact inertia tensor for this shape - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; - virtual const char* getName()const + virtual const char* getName() const { return "GImpactCompound"; } @@ -516,11 +490,8 @@ public: { 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 @@ -535,21 +506,21 @@ public: /*! Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism */ - class TrimeshPrimitiveManager:public btPrimitiveManagerBase + class TrimeshPrimitiveManager : public btPrimitiveManagerBase { public: btScalar m_margin; - btStridingMeshInterface * m_meshInterface; + btStridingMeshInterface* m_meshInterface; btVector3 m_scale; int m_part; int m_lock_count; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; - int numfaces; + int numfaces; PHY_ScalarType indicestype; TrimeshPrimitiveManager() @@ -557,7 +528,7 @@ public: m_meshInterface = NULL; m_part = 0; m_margin = 0.01f; - m_scale = btVector3(1.f,1.f,1.f); + m_scale = btVector3(1.f, 1.f, 1.f); m_lock_count = 0; vertexbase = 0; numverts = 0; @@ -567,8 +538,8 @@ public: numfaces = 0; } - TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager) - : btPrimitiveManagerBase() + TrimeshPrimitiveManager(const TrimeshPrimitiveManager& manager) + : btPrimitiveManagerBase() { m_meshInterface = manager.m_meshInterface; m_part = manager.m_part; @@ -581,11 +552,10 @@ public: indexbase = 0; indexstride = 0; numfaces = 0; - } TrimeshPrimitiveManager( - btStridingMeshInterface * meshInterface, int part) + btStridingMeshInterface* meshInterface, int part) { m_meshInterface = meshInterface; m_part = part; @@ -598,29 +568,28 @@ public: indexbase = 0; indexstride = 0; numfaces = 0; - } virtual ~TrimeshPrimitiveManager() {} void lock() { - if(m_lock_count>0) + if (m_lock_count > 0) { m_lock_count++; return; } m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase,numverts, - type, stride,&indexbase, indexstride, numfaces,indicestype,m_part); + &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) + if (m_lock_count == 0) return; + if (m_lock_count > 1) { --m_lock_count; return; @@ -637,102 +606,99 @@ public: virtual int get_primitive_count() const { - return (int )numfaces; + return (int)numfaces; } SIMD_FORCE_INLINE int get_vertex_count() const { - return (int )numverts; + return (int)numverts; } - SIMD_FORCE_INLINE void get_indices(int face_index,unsigned int &i0,unsigned int &i1,unsigned int &i2) const + SIMD_FORCE_INLINE void get_indices(int face_index, unsigned int& i0, unsigned int& i1, unsigned int& i2) const { - if(indicestype == PHY_SHORT) + if (indicestype == PHY_SHORT) { - unsigned short* s_indices = (unsigned short *)(indexbase + face_index * indexstride); + unsigned short* s_indices = (unsigned short*)(indexbase + face_index * indexstride); i0 = s_indices[0]; i1 = s_indices[1]; i2 = s_indices[2]; } + else if (indicestype == PHY_INTEGER) + { + unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride); + i0 = i_indices[0]; + i1 = i_indices[1]; + i2 = i_indices[2]; + } else { - unsigned int * i_indices = (unsigned int *)(indexbase + face_index*indexstride); + btAssert(indicestype == PHY_UCHAR); + unsigned char* i_indices = (unsigned char*)(indexbase + face_index * indexstride); i0 = i_indices[0]; i1 = i_indices[1]; i2 = i_indices[2]; } } - SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3 & vertex) const + SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const { - if(type == PHY_DOUBLE) + 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]); + 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]; + 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 + virtual void get_primitive_box(int prim_index, btAABB& primbox) const { - btPrimitiveTriangle triangle; - get_primitive_triangle(prim_index,triangle); + 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); + triangle.m_vertices[1], triangle.m_vertices[2], triangle.m_margin); } - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const { unsigned 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]); + 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 + SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index, btTriangleShapeEx& triangle) const { unsigned 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]); + 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: +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() - { - } + btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part); + virtual ~btGImpactMeshShapePart(); //! if true, then its children must get transforms. virtual bool childrenHasTransform() const @@ -740,51 +706,36 @@ public: 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(); - } + virtual void lockChildShapes() const; + virtual void unlockChildShapes() const; //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { return m_primitive_manager.get_primitive_count(); } - //! Gets the children virtual btCollisionShape* getChildShape(int index) { - (void) index; + (void)index; btAssert(0); return NULL; } - - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { - (void) index; + (void)index; btAssert(0); return NULL; } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { - (void) index; + (void)index; btAssert(0); return btTransform(); } @@ -793,35 +744,27 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { - (void) index; - (void) transform; + (void)index; + (void)transform; btAssert(0); } - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { return &m_primitive_manager; } - SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager() + SIMD_FORCE_INLINE TrimeshPrimitiveManager* getTrimeshPrimitiveManager() { return &m_primitive_manager; } + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; - - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - - - virtual const char* getName()const + virtual const char* getName() const { return "GImpactMeshShapePart"; } @@ -843,61 +786,58 @@ public: return false; } - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - m_primitive_manager.get_bullet_triangle(prim_index,triangle); + m_primitive_manager.get_bullet_triangle(prim_index, triangle); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; - (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; 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 + SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3& vertex) const { - m_primitive_manager.get_vertex(vertex_index,vertex); + 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; - virtual void processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const; -}; + { + 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; + virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const; +}; //! This class manages a mesh supplied by the btStridingMeshInterface interface. /*! @@ -913,29 +853,29 @@ class btGImpactMeshShape : public btGImpactShapeInterface protected: btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts; - void buildMeshParts(btStridingMeshInterface * meshInterface) + void buildMeshParts(btStridingMeshInterface* meshInterface) { - for (int i=0;i<meshInterface->getNumSubParts() ;++i ) + for (int i = 0; i < meshInterface->getNumSubParts(); ++i) { - btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,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()); - } - } + 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) + btGImpactMeshShape(btStridingMeshInterface* meshInterface) { m_meshInterface = meshInterface; buildMeshParts(meshInterface); @@ -944,15 +884,14 @@ public: virtual ~btGImpactMeshShape() { int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; delete part; - } + } m_mesh_parts.clear(); } - btStridingMeshInterface* getMeshInterface() { return m_meshInterface; @@ -968,79 +907,73 @@ public: return m_mesh_parts.size(); } - btGImpactMeshShapePart * getMeshPart(int index) + btGImpactMeshShapePart* getMeshPart(int index) { return m_mesh_parts[index]; } - - - const btGImpactMeshShapePart * getMeshPart(int index) const + const btGImpactMeshShapePart* getMeshPart(int index) const { return m_mesh_parts[index]; } - - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { localScaling = scaling; int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->setLocalScaling(scaling); - } + } m_needs_update = true; } virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; + { + m_collisionMargin = margin; int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + 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() - { + virtual void postUpdate() + { int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->postUpdate(); - } - - m_needs_update = true; - } + } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + m_needs_update = true; + } + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { btAssert(0); return NULL; } - //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { btAssert(0); return 0; } - //! if true, then its children must get transforms. virtual bool childrenHasTransform() const { @@ -1062,15 +995,17 @@ public: return false; } - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; btAssert(0); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } @@ -1085,39 +1020,38 @@ public: btAssert(0); } - - - //! Retrieves the bound from a child - /*! + /*! */ - virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - (void) child_index; (void) t; (void) aabbMin; (void) aabbMax; - btAssert(0); - } + virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + (void)child_index; + (void)t; + (void)aabbMin; + (void)aabbMax; + btAssert(0); + } //! Gets the children virtual btCollisionShape* getChildShape(int index) { - (void) index; + (void)index; btAssert(0); return NULL; } - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { - (void) index; + (void)index; btAssert(0); return NULL; } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { - (void) index; + (void)index; btAssert(0); return btTransform(); } @@ -1126,59 +1060,56 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { - (void) index; (void) transform; + (void)index; + (void)transform; btAssert(0); } - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const { return CONST_GIMPACT_TRIMESH_SHAPE; } - - virtual const char* getName()const + virtual const char* getName() const { return "GImpactMesh"; } - virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; + 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; + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTrianglesRay (btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const; + virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const; - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGImpactMeshShapeData +struct btGImpactMeshShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; btStridingMeshInterfaceData m_meshInterface; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; - float m_collisionMargin; + float m_collisionMargin; - int m_gimpactSubType; + int m_gimpactSubType; }; -SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const { return sizeof(btGImpactMeshShapeData); } - -#endif //GIMPACT_MESH_SHAPE_H +#endif //GIMPACT_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp index 5d07d1adb97..bfdb3db5d08 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -20,48 +20,45 @@ subject to the following restrictions: #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; + 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) + while (revindex-- && ptr == BT_UINT_MAX) { - if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements) + if (m_allocated_sizes[m_free_nodes[revindex]] >= num_elements) { ptr = revindex; } } - if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found - + 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]; + 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 + 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 + else // delete free node { // swap with end - m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1]; + m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count - 1]; m_free_nodes_count--; } @@ -70,17 +67,16 @@ size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements) size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements) { - if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX; + 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; + m_allocated_count += num_elements; return ptr; } - void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) { m_allocated_count = 0; @@ -89,14 +85,11 @@ void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) 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); - - - 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++ ) + for (size_t i = 0; i < m_max_element_count; i++) { m_allocated_sizes[i] = 0; } @@ -111,150 +104,141 @@ void btGenericMemoryPool::end_pool() 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) +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 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) + 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 + if (alloc_pos == BT_UINT_MAX) return NULL; // not space return get_element_data(alloc_pos); } -bool btGenericMemoryPool::freeMemory(void * pointer) +bool btGenericMemoryPool::freeMemory(void *pointer) { - unsigned char * pointer_pos = (unsigned char *)pointer; - unsigned char * pool_pos = (unsigned char *)m_pool; + 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 + 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 + 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[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++) + 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() +btGenericMemoryPool *btGenericPoolAllocator::push_new_pool() { - if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; + if (m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; - btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16); + 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_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) +void *btGenericPoolAllocator::failback_alloc(size_t size_bytes) { + btGenericMemoryPool *pool = NULL; - btGenericMemoryPool * pool = NULL; - - - if(size_bytes<=get_pool_capacity()) + if (size_bytes <= get_pool_capacity()) { - pool = push_new_pool(); + pool = push_new_pool(); } - if(pool==NULL) // failback + if (pool == NULL) // failback { - return btAlignedAlloc(size_bytes,16); + return btAlignedAlloc(size_bytes, 16); } return pool->allocate(size_bytes); } -bool btGenericPoolAllocator::failback_free(void * pointer) +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 *btGenericPoolAllocator::allocate(size_t size_bytes) { - void * ptr = NULL; + void *ptr = NULL; size_t i = 0; - while(i<m_pool_count && ptr == NULL) + while (i < m_pool_count && ptr == NULL) { ptr = m_pools[i]->allocate(size_bytes); ++i; } - if(ptr) return ptr; + if (ptr) return ptr; return failback_alloc(size_bytes); } -bool btGenericPoolAllocator::freeMemory(void * pointer) +bool btGenericPoolAllocator::freeMemory(void *pointer) { bool result = false; size_t i = 0; - while(i<m_pool_count && result == false) + while (i < m_pool_count && result == false) { result = m_pools[i]->freeMemory(pointer); ++i; } - if(result) return true; + 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 GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator +class GIM_STANDARD_ALLOCATOR : public btGenericPoolAllocator { public: - GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE) + GIM_STANDARD_ALLOCATOR() : btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE, BT_DEFAULT_POOL_SIZE) { } }; @@ -262,19 +246,18 @@ public: // global allocator GIM_STANDARD_ALLOCATOR g_main_allocator; - -void * btPoolAlloc(size_t size) +void *btPoolAlloc(size_t size) { return g_main_allocator.allocate(size); } -void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize) +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 *newptr = btPoolAlloc(newsize); + size_t copysize = oldsize < newsize ? oldsize : newsize; + memcpy(newptr, ptr, copysize); + btPoolFree(ptr); + return newptr; } void btPoolFree(void *ptr) diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h index b46d8516347..a535088e484 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h @@ -29,16 +29,16 @@ subject to the following restrictions: #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 + 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; @@ -47,12 +47,10 @@ protected: 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); @@ -63,10 +61,9 @@ public: end_pool(); } - inline size_t get_pool_capacity() { - return m_element_size*m_max_element_count; + return m_element_size * m_max_element_count; } inline size_t gem_element_size() @@ -89,23 +86,20 @@ public: return m_free_nodes_count; } - inline void * get_element_data(size_t element_index) + inline void *get_element_data(size_t element_index) { - return &m_pool[element_index*m_element_size]; + 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); + void *allocate(size_t size_bytes); - bool freeMemory(void * pointer); + bool freeMemory(void *pointer); }; - - - //! Generic Allocator with pools /*! General purpose Allocator which can create Memory Pools dynamiacally as needed. @@ -115,26 +109,25 @@ class btGenericPoolAllocator protected: size_t m_pool_element_size; size_t m_pool_element_count; + public: - btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS]; + 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; + return m_pool_element_size * m_pool_element_count; } - protected: // creates a pool - btGenericMemoryPool * push_new_pool(); + btGenericMemoryPool *push_new_pool(); - void * failback_alloc(size_t size_bytes); + void *failback_alloc(size_t size_bytes); - bool failback_free(void * pointer); -public: + bool failback_free(void *pointer); +public: btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count) { m_pool_count = 0; @@ -148,16 +141,13 @@ public: /*! \param size_bytes size in bytes of the buffer */ - void * allocate(size_t size_bytes); + void *allocate(size_t size_bytes); - bool freeMemory(void * pointer); + bool freeMemory(void *pointer); }; - - -void * btPoolAlloc(size_t size); -void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize); +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 index 60f06510ad7..6a1ee6dcf91 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h @@ -27,52 +27,44 @@ subject to the following restrictions: #include "btBoxCollision.h" - - - - #define PLANEDIREPSILON 0.0000001f #define PARALELENORMALS 0.000001f - -#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number)) +#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) +SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 &e1, const btVector3 &e2, const btVector3 &normal, btVector4 &plane) { - btVector3 planenormal = (e2-e1).cross(normal); + btVector3 planenormal = (e2 - e1).cross(normal); planenormal.normalize(); - plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal)); + 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 &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) + btVector3 n = e2 - e1; + cp = v - e1; + btScalar _scalar = cp.dot(n) / n.dot(n); + if (_scalar < 0.0f) { - cp = e1; + cp = e1; } - else if(_scalar >1.0f) + else if (_scalar > 1.0f) { - cp = e2; + cp = e2; } else { - cp = _scalar*n + e1; + cp = _scalar * n + e1; } } - //! line plane collision /*! *\return @@ -82,131 +74,125 @@ SIMD_FORCE_INLINE void bt_closest_point_on_segment( */ SIMD_FORCE_INLINE int bt_line_plane_collision( - const btVector4 & plane, - const btVector3 & vDir, - const btVector3 & vPoint, - btVector3 & pout, + 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) + if (btFabs(_dotdir) < PLANEDIREPSILON) { tparam = tmax; - return 0; + return 0; } - btScalar _dis = bt_distance_point_plane(plane,vPoint); - char returnvalue = _dis<0.0f? 2:1; - tparam = -_dis/_dotdir; + btScalar _dis = bt_distance_point_plane(plane, vPoint); + char returnvalue = _dis < 0.0f ? 2 : 1; + tparam = -_dis / _dotdir; - if(tparam<tmin) + if (tparam < tmin) { returnvalue = 0; tparam = tmin; } - else if(tparam>tmax) + else if (tparam > tmax) { returnvalue = 0; tparam = tmax; } - pout = tparam*vDir + vPoint; + 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) + 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)); + btVector3 AD = vA2 - vA1; + btVector3 BD = vB2 - vB1; + btVector3 N = AD.cross(BD); + btScalar tp = N.length2(); - // 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); + btVector4 _M; //plane - vPointB = tp*BD + vB1; -} + 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 +#endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h index bd2633cfc59..19a02a21772 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h @@ -27,54 +27,47 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" - - - - - SIMD_FORCE_INLINE void bt_calc_quantization_parameters( - btVector3 & outMinBound, - btVector3 & outMaxBound, - btVector3 & bvhQuantization, - const btVector3& srcMinBound,const btVector3& srcMaxBound, + 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); + 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; + 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) + 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); + 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) + const btVector3& offset, + const btVector3& bvhQuantization) { - btVector3 vecOut; + btVector3 vecOut; vecOut.setValue( (btScalar)(vecIn[0]) / (bvhQuantization.getX()), (btScalar)(vecIn[1]) / (bvhQuantization.getY()), @@ -83,6 +76,4 @@ SIMD_FORCE_INLINE btVector3 bt_unquantize( return vecOut; } - - -#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED +#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp index ca76cc54a12..292ef8c1ff3 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp @@ -23,196 +23,181 @@ subject to the following restrictions: #include "btTriangleShapeEx.h" - - -void GIM_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, - btScalar margin, const btVector3 * points, int point_count) +void GIM_TRIANGLE_CONTACT::merge_points(const btVector4& plane, + btScalar margin, const btVector3* points, int point_count) { - m_point_count = 0; - m_penetration_depth= -1000.0f; + m_point_count = 0; + m_penetration_depth = -1000.0f; - int point_indices[MAX_TRI_CLIPPING]; + 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]]; - } + 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 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 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; + 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; + 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; + // 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; + 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; + 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; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - return true; + return true; } -int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ) +int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points) { - // edge 0 - - btVector3 temp_points[MAX_TRI_CLIPPING]; - + // edge 0 - btVector4 edgeplane; + btVector3 temp_points[MAX_TRI_CLIPPING]; - get_edge_plane(0,edgeplane); + 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); + 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; + if (clipped_count == 0) return 0; - btVector3 temp_points1[MAX_TRI_CLIPPING]; + btVector3 temp_points1[MAX_TRI_CLIPPING]; + // edge 1 + get_edge_plane(1, edgeplane); - // 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; - clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1); + // edge 2 + get_edge_plane(2, edgeplane); - if (clipped_count == 0) return 0; + clipped_count = bt_plane_clip_polygon( + edgeplane, temp_points1, clipped_count, clipped_points); - // edge 2 - get_edge_plane(2,edgeplane); - - clipped_count = bt_plane_clip_polygon( - edgeplane,temp_points1,clipped_count,clipped_points); - - return clipped_count; + return clipped_count; } -bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts) +bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_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 - - GIM_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 - GIM_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; + btScalar margin = m_margin + other.m_margin; + + btVector3 clipped_points[MAX_TRI_CLIPPING]; + int clipped_count; + //create planes + // plane v vs U points + + GIM_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 + GIM_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(); + btScalar total_margin = getMargin() + other.getMargin(); - btVector4 plane0; - buildTriPlane(plane0); - btVector4 plane1; - other.buildTriPlane(plane1); + 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; + // 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 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; + 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; + 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; + // 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; + dis1 = bt_distance_point_plane(plane1, m_vertices1[1]) - total_margin; - dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - 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; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - return true; + return true; } - - diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h index 973c2ed1277..568a1ce8116 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -21,7 +21,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_TRIANGLE_SHAPE_EX_H #define GIMPACT_TRIANGLE_SHAPE_EX_H @@ -31,16 +30,15 @@ subject to the following restrictions: #include "btClipPolygon.h" #include "btGeometryOperations.h" - #define MAX_TRI_CLIPPING 16 //! Structure for collision struct GIM_TRIANGLE_CONTACT { - btScalar m_penetration_depth; - int m_point_count; - btVector4 m_separating_normal; - btVector3 m_points[MAX_TRI_CLIPPING]; + 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 GIM_TRIANGLE_CONTACT& other) { @@ -48,7 +46,7 @@ struct GIM_TRIANGLE_CONTACT m_separating_normal = other.m_separating_normal; m_point_count = other.m_point_count; int i = m_point_count; - while(i--) + while (i--) { m_points[i] = other.m_points[i]; } @@ -63,14 +61,11 @@ struct GIM_TRIANGLE_CONTACT copy_from(other); } - //! classify points that are closer - void merge_points(const btVector4 & plane, - btScalar margin, const btVector3 * points, int point_count); - + //! classify points that are closer + void merge_points(const btVector4& plane, + btScalar margin, const btVector3* points, int point_count); }; - - class btPrimitiveTriangle { public: @@ -78,17 +73,15 @@ public: btVector4 m_plane; btScalar m_margin; btScalar m_dummy; - btPrimitiveTriangle():m_margin(0.01f) + 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]); + 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)); + m_plane.setValue(normal[0], normal[1], normal[2], m_vertices[0].dot(normal)); } //! Test if triangles could collide @@ -98,14 +91,14 @@ public: /*! \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) + 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]); @@ -117,44 +110,41 @@ public: \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 ); + 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, GIM_TRIANGLE_CONTACT & contacts); + bool find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_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 +class btTriangleShapeEx : public btTriangleShape { public: - - btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0)) + 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 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]) + 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 + 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); + btAABB trianglebox(tv0, tv1, tv2, m_collisionMargin); aabbMin = trianglebox.m_min; aabbMax = trianglebox.m_max; } @@ -166,15 +156,14 @@ public: m_vertices1[2] = t(m_vertices1[2]); } - SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const + SIMD_FORCE_INLINE void buildTriPlane(btVector4& plane) const { - btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]); + 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)); + plane.setValue(normal[0], normal[1], normal[2], m_vertices1[0].dot(normal)); } bool overlap_test_conservative(const btTriangleShapeEx& other); }; - -#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H +#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h index 27e6f32fc8b..fc2dc38a330 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h @@ -34,47 +34,46 @@ email: projectileman@yahoo.com #include "gim_memory.h" - #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> +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; + //! 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) + if (newsize == 0) { destroyData(); return true; } - if(m_size>0) + if (m_size > 0) { - m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T)); + m_data = (T*)gim_realloc(m_data, m_size * sizeof(T), newsize * sizeof(T)); } else { - m_data = (T*)gim_alloc(newsize*sizeof(T)); + m_data = (T*)gim_alloc(newsize * sizeof(T)); } m_allocated_size = newsize; return true; @@ -82,243 +81,238 @@ public: inline bool growingCheck() { - if(m_allocated_size<=m_size) + 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; + 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) + //!@} + //! 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 + 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* pointer() { return m_data; } + inline const T* pointer() const + { + return m_data; + } - inline T * get_pointer_at(GUINT i) + inline T* get_pointer_at(GUINT i) { return m_data + i; } - inline const T * get_pointer_at(GUINT i) const + inline const T* get_pointer_at(GUINT i) const { return m_data + i; } - inline T & at(GUINT i) + inline T& at(GUINT i) { return m_data[i]; } - inline const T & at(GUINT i) const + inline const T& at(GUINT i) const { return m_data[i]; } - inline T & front() + inline T& front() { return *m_data; } - inline const T & front() const + inline const T& front() const { return *m_data; } - inline T & back() + inline T& back() { - return m_data[m_size-1]; + return m_data[m_size - 1]; } - inline const T & back() const + inline const T& back() const { - return m_data[m_size-1]; + return m_data[m_size - 1]; } - inline void swap(GUINT i, GUINT j) { - gim_swap_elements(m_data,i,j); + gim_swap_elements(m_data, i, j); } - inline void push_back(const T & obj) + inline void push_back(const T& obj) { - this->growingCheck(); - m_data[m_size] = obj; - m_size++; + 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++; + this->growingCheck(); + m_size++; } - inline void push_back_memcpy(const T & obj) + inline void push_back_memcpy(const T& obj) { - this->growingCheck(); - irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); - m_size++; + this->growingCheck(); + gim_simd_memcpy(&m_data[m_size], &obj, sizeof(T)); + m_size++; } inline void pop_back() { - m_size--; - m_data[m_size].~T(); + 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--; + m_size--; } - //! fast erase + //! fast erase inline void erase(GUINT index) { - if(index<m_size-1) - { - swap(index,m_size-1); - } - pop_back(); + 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)); - } + 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); + 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, const T& fillData=T()) - { - if(size>m_size) - { - reserve(size); - if(call_constructor) - { - while(m_size<size) - { - m_data[m_size] = fillData; - m_size++; - } - } - else - { - m_size = size; - } - } - else if(size<m_size) - { - if(call_constructor) clear_range(size); - m_size = size; - } + 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 refit() + inline void insert(const T& obj, GUINT index) { - resizeData(m_size); + insert_mem(index); + m_data[index] = obj; } -}; - - - + inline void resize(GUINT size, bool call_constructor = true, const T& fillData = T()) + { + if (size > m_size) + { + reserve(size); + if (call_constructor) + { + while (m_size < size) + { + m_data[m_size] = fillData; + 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 +#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 index d98051da3d6..7ab783672d6 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -35,88 +35,92 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_linear_math.h" - - - - +#ifndef PLANEDIREPSILON #define PLANEDIREPSILON 0.0000001f -#define PARALELENORMALS 0.000001f - +#endif -#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);\ -}\ +#ifndef PARALELENORMALS +#define PARALELENORMALS 0.000001f +#endif + +#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); \ -}\ +#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);\ -}\ +#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);\ -}\ +#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); \ -}\ +#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> +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) + const CLASS_POINT &point, const CLASS_PLANE *planes, GUINT plane_count) { GREAL _dis; - for (GUINT _i = 0;_i< plane_count;++_i) + for (GUINT _i = 0; _i < plane_count; ++_i) { - _dis = DISTANCE_PLANE_POINT(planes[_i],point); - if(_dis>0.0f) return false; + _dis = DISTANCE_PLANE_POINT(planes[_i], point); + if (_dis > 0.0f) return false; } return true; } -template<typename CLASS_POINT,typename CLASS_PLANE> +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) + 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); + 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 @@ -149,30 +153,30 @@ intersection type must have the following values </ul> */ -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2( - const CLASS_POINT& s1, + const CLASS_POINT &s1, const CLASS_POINT &s2, - const CLASS_PLANE &plane,CLASS_POINT &clipped) + 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) + 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; + if (_dis1 < _dis2) return G_FRONT_PLANE_S1; + return G_FRONT_PLANE_S2; } - else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON) + else if (_dis1 < G_EPSILON && _dis2 < G_EPSILON) { - if(_dis1>_dis2) return G_BACK_PLANE_S1; - return G_BACK_PLANE_S2; + 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; + 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; } @@ -191,43 +195,42 @@ intersection_type must have the following values <li> 5 : Segment collides plane, s2 in back </ul> */ -template<typename CLASS_POINT,typename CLASS_PLANE> +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 &s1, const CLASS_POINT &s2, const CLASS_PLANE &plane, - CLASS_POINT &clipped1,CLASS_POINT &clipped2) + CLASS_POINT &clipped1, CLASS_POINT &clipped2) { - eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1); - switch(intersection_type) + 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; + 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) @@ -236,23 +239,23 @@ SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( 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> +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) + 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) + GREAL _dis, _dotdir; + _dotdir = VEC_DOT(plane, vDir); + if (_dotdir < PLANEDIREPSILON) { - return false; + return false; } - _dis = DISTANCE_PLANE_POINT(plane,vPoint); - tparam = -_dis/_dotdir; - VEC_SCALE(pout,tparam,vDir); - VEC_SUM(pout,vPoint,pout); + _dis = DISTANCE_PLANE_POINT(plane, vPoint); + tparam = -_dis / _dotdir; + VEC_SCALE(pout, tparam, vDir); + VEC_SUM(pout, vPoint, pout); return true; } @@ -263,39 +266,39 @@ SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION( -1 if the ray collides in front -2 if the ray collides in back */ -template<typename T,typename CLASS_POINT,typename CLASS_PLANE> +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, + 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) + GREAL _dis, _dotdir; + _dotdir = VEC_DOT(plane, vDir); + if (btFabs(_dotdir) < PLANEDIREPSILON) { tparam = tmax; - return 0; + return 0; } - _dis = DISTANCE_PLANE_POINT(plane,vPoint); - char returnvalue = _dis<0.0f?2:1; - tparam = -_dis/_dotdir; + _dis = DISTANCE_PLANE_POINT(plane, vPoint); + char returnvalue = _dis < 0.0f ? 2 : 1; + tparam = -_dis / _dotdir; - if(tparam<tmin) + if (tparam < tmin) { returnvalue = 0; tparam = tmin; } - else if(tparam>tmax) + else if (tparam > tmax) { returnvalue = 0; tparam = tmax; } - VEC_SCALE(pout,tparam,vDir); - VEC_SUM(pout,vPoint,pout); + VEC_SCALE(pout, tparam, vDir); + VEC_SUM(pout, vPoint, pout); return returnvalue; } @@ -309,24 +312,24 @@ SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( \return true if the planes intersect, 0 if paralell. */ -template<typename CLASS_POINT,typename CLASS_PLANE> +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) + 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; + 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; + _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; } @@ -334,32 +337,31 @@ SIMD_FORCE_INLINE bool INTERSECT_PLANES( /*! Finds the closest point(cp) to (v) on a segment (e1,e2) */ -template<typename CLASS_POINT> +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) + 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); + 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) + _scalar /= VEC_DOT(_n, _n); + if (_scalar < 0.0f) { - VEC_COPY(cp,e1); + VEC_COPY(cp, e1); } - else if(_scalar >1.0f) + else if (_scalar > 1.0f) { - VEC_COPY(cp,e2); + VEC_COPY(cp, e2); } else { - VEC_SCALE(cp,_scalar,_n); - VEC_SUM(cp,cp,e1); + 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 @@ -371,117 +373,113 @@ SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( \param dointersect 0 if the lines won't intersect, else 1 */ -template<typename T,typename CLASS_POINT> +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) + 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); + 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; + 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> +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) + 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); -} - + 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 /*! @@ -494,37 +492,36 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION( *\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) +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)) + if (GIM_IS_ZERO(dir)) { - return !(pos < bmin || pos > bmax); + return !(pos < bmin || pos > bmax); } GREAL a0 = (bmin - pos) / dir; GREAL a1 = (bmax - pos) / dir; - if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1); + 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> +template <typename T> SIMD_FORCE_INLINE void SORT_3_INDICES( - const T * values, - GUINT * order_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; + GUINT i0 = (order_indices[0] + 1) % 3; + GUINT i1 = (i0 + 1) % 3; - if(values[i0] < values[i1]) + if (values[i0] < values[i1]) { order_indices[1] = i0; order_indices[2] = i1; @@ -536,8 +533,4 @@ SIMD_FORCE_INLINE void SORT_3_INDICES( } } - - - - -#endif // GIM_VECTOR_H_INCLUDED +#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 index 7dee48a4c76..c1fb41a5c0c 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h @@ -34,34 +34,32 @@ email: projectileman@yahoo.com #include "gim_array.h" - #define GUINT_BIT_COUNT 32 #define GUINT_EXPONENT 5 class gim_bitset { public: - gim_array<GUINT> m_container; - - gim_bitset() - { + gim_array<GUINT> m_container; - } + gim_bitset() + { + } - gim_bitset(GUINT bits_count) - { - resize(bits_count); - } + gim_bitset(GUINT bits_count) + { + resize(bits_count); + } - ~gim_bitset() - { - } + ~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.resize(newsize / GUINT_BIT_COUNT + 1, false); + while (oldsize < m_container.size()) { m_container[oldsize] = 0; } @@ -70,12 +68,12 @@ public: inline GUINT size() { - return m_container.size()*GUINT_BIT_COUNT; + return m_container.size() * GUINT_BIT_COUNT; } inline void set_all() { - for(GUINT i = 0;i<m_container.size();++i) + for (GUINT i = 0; i < m_container.size(); ++i) { m_container[i] = 0xffffffff; } @@ -83,7 +81,7 @@ public: inline void clear_all() { - for(GUINT i = 0;i<m_container.size();++i) + for (GUINT i = 0; i < m_container.size(); ++i) { m_container[i] = 0; } @@ -91,33 +89,29 @@ public: inline void set(GUINT bit_index) { - if(bit_index>=size()) + if (bit_index >= size()) { resize(bit_index); } - m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1))); + 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()) + if (bit_index >= size()) { return 0; } char value = m_container[bit_index >> GUINT_EXPONENT] & - (1 << (bit_index & (GUINT_BIT_COUNT-1))); + (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))); + m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT - 1))); } }; - - - - -#endif // GIM_CONTAINERS_H_INCLUDED +#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 index 9c572638acd..9f7cbe732fa 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h @@ -35,8 +35,6 @@ email: projectileman@yahoo.com #include "gim_basic_geometry_operations.h" #include "LinearMath/btTransform.h" - - //SIMD_FORCE_INLINE bool test_cross_edge_box( // const btVector3 & edge, // const btVector3 & absolute_edge, @@ -97,51 +95,52 @@ email: projectileman@yahoo.com // 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);\ -}\ +#ifndef TEST_CROSS_EDGE_BOX_MCR + +#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_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\ -}\ +#endif -#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);\ -}\ +#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 + 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); + 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(); @@ -151,47 +150,46 @@ public: { } - - GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0) + 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) + 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) + 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_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) + 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_T1to0 += m_R1to0 * trans1.getOrigin(); m_R1to0 *= trans1.getBasis(); calc_absolute_matrix(); } - SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) + SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point) { - return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; + return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; } }; - +#ifndef BOX_PLANE_EPSILON #define BOX_PLANE_EPSILON 0.000001f +#endif //! Axis aligned box class GIM_AABB @@ -201,34 +199,34 @@ public: btVector3 m_max; GIM_AABB() - {} - + { + } - GIM_AABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3) + 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_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_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, + 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_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_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; @@ -238,13 +236,11 @@ public: 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) : 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) + 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; @@ -285,34 +281,34 @@ public: m_max[2] = other.m_max[2] + margin; } - template<typename CLASS_POINT> + template <typename CLASS_POINT> SIMD_FORCE_INLINE void calc_from_triangle( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3) + 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_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_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> + 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) + 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_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_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; @@ -323,74 +319,73 @@ public: } //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + SIMD_FORCE_INLINE void appy_transform(const btTransform &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans(center); - btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), - trans.getBasis().getRow(1).absolute(), - trans.getBasis().getRow(2).absolute()); - + btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), + trans.getBasis().getRow(1).absolute(), + 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) + 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_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]); + 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) + 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_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]); + 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 + SIMD_FORCE_INLINE void get_center_extend(btVector3 ¢er, btVector3 &extend) const { - center = (m_max+m_min)*0.5f; + 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 + 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_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]); + 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 + 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]) + 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; } @@ -402,35 +397,34 @@ public: \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) + SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) { - btVector3 extents,center; - this->get_center_extend(center,extents);; + 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; + 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; + 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; - + 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; + 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; + 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; + 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 + 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; + btVector3 center = (m_max + m_min) * 0.5f; + btVector3 extend = m_max - center; - btScalar _fOrigin = direction.dot(center); + btScalar _fOrigin = direction.dot(center); btScalar _fMaximumExtent = extend.dot(direction.absolute()); vmin = _fOrigin - _fMaximumExtent; vmax = _fOrigin + _fMaximumExtent; @@ -438,22 +432,22 @@ public: SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const { - btScalar _fmin,_fmax; - this->projection_interval(plane,_fmin,_fmax); + btScalar _fmin, _fmax; + this->projection_interval(plane, _fmin, _fmax); - if(plane[3] > _fmax + BOX_PLANE_EPSILON) + if (plane[3] > _fmax + BOX_PLANE_EPSILON) { - return G_BACK_PLANE; // 0 + return G_BACK_PLANE; // 0 } - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + if (plane[3] + BOX_PLANE_EPSILON >= _fmin) { - return G_COLLIDE_PLANE; //1 + return G_COLLIDE_PLANE; //1 } - return G_FRONT_PLANE;//2 + return G_FRONT_PLANE; //2 } - SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0) + 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); @@ -462,52 +456,50 @@ public: //! 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) + 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 ea, eb; //extends + btVector3 ca, cb; //extends + get_center_extend(ca, ea); + box.get_center_extend(cb, eb); btVector3 T; - btScalar t,t2; + btScalar t, t2; int i; // Class I : A's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[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; + if (GIM_GREATER(T[i], t)) return false; } // Class II : B's basis vectors - for(i=0;i<3;i++) + 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; + 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) + if (fulltest) { - int j,m,n,o,p,q,r; - for(i=0;i<3;i++) + 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++) + 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; + 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; } } } @@ -516,7 +508,7 @@ public: //! Simple test for planes. SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) + const btVector4 &plane) { ePLANE_INTERSECTION_TYPE classify = plane_classify(plane); return (classify == G_COLLIDE_PLANE); @@ -524,15 +516,15 @@ public: //! 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 btVector3 &p1, + const btVector3 &p2, + const btVector3 &p3, + const btVector4 &triangle_plane) { - if(!collide_plane(triangle_plane)) return false; + if (!collide_plane(triangle_plane)) return false; - btVector3 center,extends; - this->get_center_extend(center,extends); + btVector3 center, extends; + this->get_center_extend(center, extends); const btVector3 v1(p1 - center); const btVector3 v2(p2 - center); @@ -542,47 +534,45 @@ public: 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_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_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); - + 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_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_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); + 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_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_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); + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v3, v2, extends); return true; } }; - +#ifndef BT_BOX_COLLISION_H_INCLUDED //! Compairison of transformation objects -SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2) { - if(!(t1.getOrigin() == t2.getOrigin()) ) return false; + 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; + 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 - - -#endif // GIM_BOX_COLLISION_H_INCLUDED +#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 index 0c3d7ba8db0..0c7a6b7fc1b 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp @@ -28,67 +28,64 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_box_set.h" - GUINT GIM_BOX_TREE::_calc_splitting_axis( - gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex) + 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; + 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++) + 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; + 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); + means *= (btScalar(1.) / (btScalar)numIndices); - for (i=startIndex;i<endIndex;i++) + 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; + 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) ); + 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, + gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex, GUINT splitAxis) { GUINT i; - GUINT splitIndex =startIndex; + GUINT splitIndex = startIndex; GUINT numIndices = endIndex - startIndex; // average of centers btScalar splitValue = 0.0f; - for (i=startIndex;i<endIndex;i++) + 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 += 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++) + 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]); + 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); + primitive_boxes.swap(i, splitIndex); splitIndex++; } } @@ -102,28 +99,27 @@ GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - GUINT rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + GUINT rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; } - -void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex) +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); + btAssert((endIndex - startIndex) > 0); - if((endIndex-startIndex) == 1) //we got a leaf - { + 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; @@ -138,8 +134,8 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G 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.invalidate(); + for (splitIndex = startIndex; splitIndex < endIndex; splitIndex++) { m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound); } @@ -147,36 +143,34 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G //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 = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex,splitIndex); + 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 ); + _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); + _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; + 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) + 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); - + 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 index 61d190a7dfe..afc591dac0d 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h @@ -33,54 +33,30 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_array.h" #include "gim_radixsort.h" #include "gim_box_collision.h" #include "gim_tri_collision.h" - - - -//! 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; - } -}; +#include "gim_pair.h" //! A pairset array -class gim_pair_set: public gim_array<GIM_PAIR> +class gim_pair_set : public gim_array<GIM_PAIR> { public: - gim_pair_set():gim_array<GIM_PAIR>(32) + gim_pair_set() : gim_array<GIM_PAIR>(32) { } - inline void push_pair(GUINT index1,GUINT index2) + inline void push_pair(GUINT index1, GUINT index2) { - push_back(GIM_PAIR(index1,index2)); + push_back(GIM_PAIR(index1, index2)); } - inline void push_pair_inv(GUINT index1,GUINT index2) + inline void push_pair_inv(GUINT index1, GUINT index2) { - push_back(GIM_PAIR(index2,index1)); + push_back(GIM_PAIR(index2, index1)); } }; - //! Prototype Base class for primitive classification /*! This class is a wrapper for primitive collections. @@ -90,16 +66,14 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime class GIM_PRIMITIVE_MANAGER_PROTOTYPE { public: - virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {} //! 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; + 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; @@ -110,22 +84,22 @@ struct GIM_AABB_DATA 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 + 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; + 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); + return (!m_left && !m_right); } }; @@ -135,14 +109,16 @@ 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); + 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); + 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); - void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex); public: GIM_BOX_TREE() { @@ -151,7 +127,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes); + void build_tree(gim_array<GIM_AABB_DATA>& primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -176,22 +152,22 @@ public: return m_node_array[nodeindex].m_data; } - SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + 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) + 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 + SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const { return m_node_array[nodeindex].m_left; } - SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const + SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const { return m_node_array[nodeindex].m_right; } @@ -204,78 +180,78 @@ public: //!@} }; - //! 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> +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--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { GIM_AABB leafbox; - m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,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); + getNodeBound(childindex, bound); //get right bound childindex = getRightNodeIndex(nodecount); GIM_AABB bound2; - getNodeBound(childindex,bound2); + getNodeBound(childindex, bound2); bound.merge(bound2); - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } -public: +public: GIM_BOX_TREE_TEMPLATE_SET() { } - SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const + 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) + SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& primitive_manager) { m_primitive_manager = primitive_manager; } - const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const + const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() const { return m_primitive_manager; } - _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() + _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() { return m_primitive_manager; } -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -288,19 +264,19 @@ public: { //obtain primitive boxes gim_array<GIM_AABB_DATA> primitive_boxes; - primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false); + primitive_boxes.resize(m_primitive_manager.get_primitive_count(), false); - for (GUINT i = 0;i<primitive_boxes.size() ;i++ ) + 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_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 + SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB& box, gim_array<GUINT>& collided_results) const { GUINT curIndex = 0; GUINT numNodes = getNodeCount(); @@ -308,7 +284,7 @@ public: while (curIndex < numNodes) { GIM_AABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data @@ -328,26 +304,26 @@ public: else { //skip node - curIndex+= getScapeNodeIndex(curIndex); + curIndex += getScapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + 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 + SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB& box, + const btTransform& transform, gim_array<GUINT>& collided_results) const { - GIM_AABB transbox=box; + GIM_AABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + 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 + const btVector3& ray_dir, const btVector3& ray_origin, + gim_array<GUINT>& collided_results) const { GUINT curIndex = 0; GUINT numNodes = getNodeCount(); @@ -355,16 +331,16 @@ public: while (curIndex < numNodes) { GIM_AABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -375,10 +351,10 @@ public: else { //skip node - curIndex+= getScapeNodeIndex(curIndex); + curIndex += getScapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } @@ -389,7 +365,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager.is_trimesh(); } @@ -411,12 +387,12 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + 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) + SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound) { m_box_tree.setNodeBound(nodeindex, bound); } @@ -436,36 +412,30 @@ public: return m_box_tree.getScapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex, GIM_TRIANGLE& triangle) const { - m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle); + 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> +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> +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; + 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; @@ -483,18 +453,18 @@ public: 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); + 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]); @@ -506,8 +476,8 @@ protected: SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1) { - if(node1_has_triangle) return; - m_boxset1->getNodeTriangle(node1,m_tri1); + 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]); @@ -519,8 +489,8 @@ protected: SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0) { - if(node0 == current_node0) return; - m_boxset0->getNodeBound(node0,m_box0); + 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; @@ -528,21 +498,21 @@ protected: SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1) { - if(node1 == current_node1) return; - m_boxset1->getNodeBound(node1,m_box1); + 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) + 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; + bool result = m_box0.overlapping_trans_cache(m_box1, trans_cache_1to0, true); + if (!result) return false; - if(t0_is_trimesh && node0_is_leaf) + if (t0_is_trimesh && node0_is_leaf) { //perform primitive vs box collision retrieve_node0_triangle(node0); @@ -550,14 +520,14 @@ protected: 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_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; + if (!result) return false; return true; } - else if(t1_is_trimesh && node1_is_leaf) + else if (t1_is_trimesh && node1_is_leaf) { //perform primitive vs box collision retrieve_node1_triangle(node1); @@ -565,11 +535,11 @@ protected: 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_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; + if (!result) return false; return true; } return true; @@ -582,40 +552,39 @@ protected: stack_collisions.reserve(32); //add the first pair - stack_collisions.push_pair(0,0); + stack_collisions.push_pair(0, 0); - - while(stack_collisions.size()) + 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 (node_collision(node0, node1)) // a collision is found { - if(node0_is_leaf) + if (node0_is_leaf) { - if(node1_is_leaf) + if (node1_is_leaf) { - m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1)); + 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)); + stack_collisions.push_pair(node0, m_boxset1->getLeftNodeIndex(node1)); //collide right - stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1)); + stack_collisions.push_pair(node0, m_boxset1->getRightNodeIndex(node1)); } } else { - if(node1_is_leaf) + if (node1_is_leaf) { //collide left - stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1); + stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0), node1); //collide right - stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1); + stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0), node1); } else { @@ -624,36 +593,36 @@ protected: GUINT left1 = m_boxset1->getLeftNodeIndex(node1); GUINT right1 = m_boxset1->getRightNodeIndex(node1); //collide left - stack_collisions.push_pair(left0,left1); + stack_collisions.push_pair(left0, left1); //collide right - stack_collisions.push_pair(left0,right1); + stack_collisions.push_pair(left0, right1); //collide left - stack_collisions.push_pair(right0,left1); + stack_collisions.push_pair(right0, left1); //collide right - stack_collisions.push_pair(right0,right1); + stack_collisions.push_pair(right0, right1); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf - }// if(node_collision(node0,node1)) - }//while(stack_collisions.size()) + } // 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) + 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_1to0.calc_from_homogenic(trans1, trans2); - trans_cache_0to1 = trans2.inverse(); + trans_cache_0to1 = trans2.inverse(); trans_cache_0to1 *= trans1; - - if(complete_primitive_tests) + if (complete_primitive_tests) { t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh(); t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh(); @@ -668,7 +637,4 @@ public: } }; - -#endif // GIM_BOXPRUNING_H_INCLUDED - - +#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 index e342459ce56..57b9c5c91ad 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h @@ -33,91 +33,86 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - //! 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) + 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> +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) + 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) + 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); + GREAL blendfactor = -dist0 / (dist1 - dist0); + VEC_BLEND(clipped[clipped_count], point0, point1, blendfactor); clipped_count++; } - if(!_classif) + if (!_classif) { - VEC_COPY(clipped[clipped_count],point1); + 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> +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) + 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; + GUINT clipped_count = 0; - - //clip first point - GREAL firstdist = distance_func(plane,polygon_points[0]);; - if(!(firstdist>G_EPSILON)) + //clip first point + GREAL firstdist = distance_func(plane, polygon_points[0]); + ; + if (!(firstdist > G_EPSILON)) { - VEC_COPY(clipped[clipped_count],polygon_points[0]); + 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]); + 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); + polygon_points[_i - 1], polygon_points[_i], + olddist, + dist, + clipped, + clipped_count); - - olddist = dist; + olddist = dist; } - //RETURN TO FIRST point + //RETURN TO FIRST point PLANE_CLIP_POLYGON_COLLECT( - polygon_points[polygon_point_count-1],polygon_points[0], - olddist, - firstdist, - clipped, - clipped_count); + polygon_points[polygon_point_count - 1], polygon_points[0], + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } @@ -126,85 +121,79 @@ SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC( /*! *\return The count of the clipped counts */ -template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC> +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) + 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; + GUINT clipped_count = 0; - //clip first point - GREAL firstdist = distance_func(plane,point0);; - if(!(firstdist>G_EPSILON)) + //clip first point + GREAL firstdist = distance_func(plane, point0); + ; + if (!(firstdist > G_EPSILON)) { - VEC_COPY(clipped[clipped_count],point0); + VEC_COPY(clipped[clipped_count], point0); clipped_count++; } // point 1 GREAL olddist = firstdist; - GREAL dist = distance_func(plane,point1); + GREAL dist = distance_func(plane, point1); PLANE_CLIP_POLYGON_COLLECT( - point0,point1, - olddist, - dist, - clipped, - clipped_count); + point0, point1, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - // point 2 - dist = distance_func(plane,point2); + dist = distance_func(plane, point2); PLANE_CLIP_POLYGON_COLLECT( - point1,point2, - olddist, - dist, - clipped, - clipped_count); + point1, point2, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - - //RETURN TO FIRST point PLANE_CLIP_POLYGON_COLLECT( - point2,point0, - olddist, - firstdist, - clipped, - clipped_count); + point2, point0, + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } - -template<typename CLASS_POINT,typename CLASS_PLANE> +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) + 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()); + 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> +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) + 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()); + return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT, CLASS_PLANE>(plane, point0, point1, point2, clipped, DISTANCE_PLANE_3D_FUNC()); } - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#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 index 20e41de089f..390225709e1 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp @@ -33,91 +33,89 @@ email: projectileman@yahoo.com #define MAX_COINCIDENT 8 void gim_contact_array::merge_contacts( - const gim_contact_array & contacts, bool normal_contact_average) + const gim_contact_array& contacts, bool normal_contact_average) { clear(); - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts.back()); return; } gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size()); - keycontacts.resize(contacts.size(),false); + keycontacts.resize(contacts.size(), false); //fill key contacts GUINT i; - for (i = 0;i<contacts.size() ;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()); + gim_heap_sort(keycontacts.pointer(), keycontacts.size(), GIM_RSORT_TOKEN_COMPARATOR()); // Merge contacts - GUINT coincident_count=0; + 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(); + GIM_CONTACT* pcontact = &back(); - - - for( i=1;i<keycontacts.size();i++) + for (i = 1; i < keycontacts.size(); i++) { - key = keycontacts[i].m_key; - const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; + key = keycontacts[i].m_key; + const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value]; - if(last_key == key)//same points + if (last_key == key) //same points { //merge contact - if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //) { *pcontact = *scontact; - coincident_count = 0; + coincident_count = 0; } - else if(normal_contact_average) + 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++; - } - } + 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 + { //add new contact - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } + if (normal_contact_average && coincident_count > 0) + { + pcontact->interpolate_normals(coincident_normals, coincident_count); + coincident_count = 0; + } - push_back(*scontact); - pcontact = &back(); - } + push_back(*scontact); + pcontact = &back(); + } last_key = key; } } -void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts) +void gim_contact_array::merge_contacts_unique(const gim_contact_array& contacts) { clear(); - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts.back()); return; @@ -125,14 +123,14 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts GIM_CONTACT average_contact = contacts.back(); - for (GUINT i=1;i<contacts.size() ;i++ ) + 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()); + GREAL divide_average = 1.0f / ((GREAL)contacts.size()); average_contact.m_point *= divide_average; @@ -141,6 +139,4 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts 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 index 5d9f8ef8180..9deb28a26eb 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h @@ -36,12 +36,18 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" #include "gim_array.h" - /** Configuration var for applying interpolation of contact normals */ +#ifndef NORMAL_CONTACT_AVERAGE #define NORMAL_CONTACT_AVERAGE 1 +#endif + +#ifndef CONTACT_DIFF_EPSILON #define CONTACT_DIFF_EPSILON 0.00001f +#endif + +#ifndef BT_CONTACT_H_STRUCTS_INCLUDED /// Structure for collision results ///Functions for managing and sorting contacts resulting from a collision query. @@ -51,89 +57,87 @@ Configuration var for applying interpolation of contact normals 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 + 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) - { - } + 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; + 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; + _hash += (*_uitmp) << 4; _uitmp++; - _hash += (*_uitmp)<<8; + _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++) + 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; + 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; - } + GIM_INV_SQRT(vec_sum_len, vec_sum_len); // 1/sqrt(vec_sum_len) + m_normal = vec_sum * vec_sum_len; + } }; +#endif -class gim_contact_array:public gim_array<GIM_CONTACT> +class gim_contact_array : public gim_array<GIM_CONTACT> { public: - gim_contact_array():gim_array<GIM_CONTACT>(64) + 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) + 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(); + GIM_CONTACT &newele = back(); newele.m_point = point; newele.m_normal = normal; newele.m_depth = depth; @@ -142,13 +146,13 @@ public: } SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT_DATA & tricontact, - GUINT feature1,GUINT feature2) + const GIM_TRIANGLE_CONTACT_DATA &tricontact, + GUINT feature1, GUINT feature2) { - for(GUINT i = 0;i<tricontact.m_point_count ;i++ ) + for (GUINT i = 0; i < tricontact.m_point_count; i++) { push_back_mem(); - GIM_CONTACT & newele = back(); + 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; @@ -157,8 +161,8 @@ public: } } - void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true); - void merge_contacts_unique(const gim_contact_array & contacts); + 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 +#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 index 6b8f9ea6c21..9dc48f354b9 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h @@ -33,11 +33,8 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_math.h" - - //! Short Integer vector 2D typedef GSHORT vec2s[2]; //! Integer vector 3D @@ -92,6 +89,4 @@ typedef GREAL quatf[4]; //typedef struct _aabb3f aabb3f; - - -#endif // GIM_GEOM_TYPES_H_INCLUDED +#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 index c67a6991c02..4a7ac3c4d80 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h @@ -39,4 +39,4 @@ email: projectileman@yahoo.com #include "gim_box_collision.h" #include "gim_tri_collision.h" -#endif // GIM_VECTOR_H_INCLUDED +#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 index e4237c2c570..abf88d3108d 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h @@ -34,53 +34,52 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" - -#define GIM_INVALID_HASH 0xffffffff //!< A very very high value +#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 +#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII -template<typename T> +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 + 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; + if (m_key < other.m_key) return true; return false; } - bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const + bool operator>(const GIM_HASH_TABLE_NODE<T>& other) const { ///inverse order, further objects are first - if(m_key > other.m_key) return true; + if (m_key > other.m_key) return true; return false; } - bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const + bool operator==(const GIM_HASH_TABLE_NODE<T>& other) const { ///inverse order, further objects are first - if(m_key == other.m_key) return true; + if (m_key == other.m_key) return true; return false; } }; @@ -89,21 +88,19 @@ struct GIM_HASH_TABLE_NODE class GIM_HASH_NODE_GET_KEY { public: - template<class T> - inline GUINT operator()( const T& a) + 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) + template <class T> + inline int operator()(const T& a, GUINT key) { return ((int)(a.m_key - key)); } @@ -113,65 +110,53 @@ public: class GIM_HASH_NODE_CMP_MACRO { public: - template<class T> - inline int operator() ( const T& a, const T& b ) + 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) +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); - } + 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 -}; + { + 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); + //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]; + // 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. @@ -187,129 +172,124 @@ When the array size reaches the size equivalent to 'min_hash_table_size', then i </ul> */ -template<class T> +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; - - ///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. - /*! + 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; + + ///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); + 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 @@ -318,585 +298,560 @@ protected: //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 - /*! + } + } + } + + //! 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; - } + 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 cell_index = _assign_hash_table_cell(hashkey); - GUINT value_key = m_hash_table[cell_index]; + GUINT value_key = m_hash_table[cell_index]; - if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited + if (value_key != GIM_INVALID_HASH) return value_key; // Not overrited - m_hash_table[cell_index] = m_nodes.size(); + m_hash_table[cell_index] = m_nodes.size(); - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } - //! insert an element in hash table. - /*! + //! 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; - - } - - - ///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) + 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; + } + + ///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); + 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 - /*! + } + 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; - } - - + 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(); + } -public: + //! 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 - /*! + 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 - /*! + 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]; - } + 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; + 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(); + _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()); + 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; + } - if(found) return result_ind; - } - return GIM_INVALID_HASH; - } - - //! Retrieves the value associated with the index - /*! + //! 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 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_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 - } + 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 - /*! + //! 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. - /*! + 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); - } - + 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 +#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 index 64f11b49543..98401a404ab 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h @@ -34,962 +34,900 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_math.h" #include "gim_geom_types.h" - - - //! Zero out a 2D vector -#define VEC_ZERO_2(a) \ -{ \ - (a)[0] = (a)[1] = 0.0f; \ -}\ - +#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; \ -}\ - +#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; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ +#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]);\ -}\ +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]) - +#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]) +#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]) +#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_;\ -}\ - +#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); \ -}\ +#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);\ -}\ - +#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);\ -}\ - +#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);\ -}\ +#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);\ -}\ - +#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);\ -}\ - +#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);\ -}\ - - +#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); \ -}\ - +#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);\ -}\ - +#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; \ - } \ -}\ +#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; \ - } \ -}\ +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ +#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_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]; +#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);\ -}\ +#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_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])) +#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; \ -}\ - - - - - +#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; \ + } /// 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; \ -}\ +#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; \ -}\ +#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; \ -}\ +#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; \ -}\ +#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; \ -}\ +#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; \ -}\ +#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]; \ - \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ +#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]; \ - \ -}\ +#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]; \ -}\ - +#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];\ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ +#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];\ -}\ - - +#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 * @@ -1000,22 +938,22 @@ Last column is added as the position * 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; \ - } \ -}\ - +#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 @@ -1024,550 +962,527 @@ Last column is added as the position * 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); \ - } \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]; \ -}\ - +#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]); \ -}\ - +#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); \ - } \ -}\ - +#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; \ -}\ - +#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]; \ -}\ - +#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]); \ -}\ - +#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); \ - } \ - } \ -}\ - +#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]; \ -}\ - +#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]); \ -}\ - +#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_); \ - } \ - } \ -}\ - +#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]; \ -}\ - +#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]); \ -}\ - +#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; \ - } \ - } \ -}\ +#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); \ -}\ - +#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); \ -}\ - +#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); \ -}\ +#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]; \ -}\ +#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];\ -}\ - +#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];\ -}\ +#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]; \ -}\ +#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];\ -}\ - +#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];\ -}\ +#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);\ -}\ +#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);\ -}\ +#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);\ -}\ - +#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];\ -}\ +#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];\ -}\ +#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];\ -}\ - +#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]; \ -}\ +#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]; \ -}\ - - +#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]) +#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]) +#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]) - +#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]) +#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]) +#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]) +#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 +#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 index 939079e1040..3c4f821a722 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h @@ -34,8 +34,6 @@ email: projectileman@yahoo.com #include "LinearMath/btScalar.h" - - #define GREAL btScalar #define GREAL2 double #define GINT int @@ -45,8 +43,6 @@ email: projectileman@yahoo.com #define GINT64 long long #define GUINT64 unsigned long long - - #define G_PI 3.14159265358979f #define G_HALF_PI 1.5707963f //267948966 @@ -54,16 +50,14 @@ email: projectileman@yahoo.com //71795864 #define G_ROOT3 1.73205f #define G_ROOT2 1.41421f -#define G_UINT_INFINITY 0xffffffff //!< A very very high value +#define G_UINT_INFINITY 0xffffffff //!< A very very high value #define G_REAL_INFINITY FLT_MAX -#define G_SIGN_BITMASK 0x80000000 +#define G_SIGN_BITMASK 0x80000000 #define G_EPSILON SIMD_EPSILON - - enum GIM_SCALAR_TYPES { - G_STYPE_REAL =0, + G_STYPE_REAL = 0, G_STYPE_REAL2, G_STYPE_SHORT, G_STYPE_USHORT, @@ -73,85 +67,82 @@ enum GIM_SCALAR_TYPES G_STYPE_UINT64 }; - - -#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f) -#define G_RADTODEG(X) ((X)*180.0f/3.1415926f) +#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)) +#define GIM_IR(x) ((GUINT&)(x)) //! Signed integer representation of a floating-point value. -#define GIM_SIR(x) ((GINT&)(x)) +#define GIM_SIR(x) ((GINT&)(x)) //! Absolute integer representation of a floating-point value -#define GIM_AIR(x) (GIM_IR(x)&0x7fffffff) +#define GIM_AIR(x) (GIM_IR(x) & 0x7fffffff) //! Floating-point representation of an integer value. -#define GIM_FR(x) ((GREAL&)(x)) +#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_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_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_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)) +#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_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number)) -#define GIM_GREATER(x, y) btFabs(x) > (y) +#define GIM_GREATER(x, y) btFabs(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;\ -}\ +#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; + GREAL r; + GIM_INV_SQRT(f, r); + return r; } inline GREAL gim_sqrt(GREAL f) { - GREAL r; - GIM_SQRT(f,r); - return r; + GREAL r; + GIM_SQRT(f, r); + return r; } - - -#endif // GIM_MATH_H_INCLUDED +#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 index 1636eb7867c..9e29ab91d67 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp @@ -27,7 +27,6 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_memory.h" #include "stdlib.h" @@ -40,52 +39,49 @@ 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) +void gim_set_alloc_handler(gim_alloc_function *fn) { - g_allocfn = fn; + g_allocfn = fn; } -void gim_set_alloca_handler (gim_alloca_function *fn) +void gim_set_alloca_handler(gim_alloca_function *fn) { - g_allocafn = fn; + g_allocafn = fn; } -void gim_set_realloc_handler (gim_realloc_function *fn) +void gim_set_realloc_handler(gim_realloc_function *fn) { - g_reallocfn = fn; + g_reallocfn = fn; } -void gim_set_free_handler (gim_free_function *fn) +void gim_set_free_handler(gim_free_function *fn) { - g_freefn = fn; + g_freefn = fn; } gim_alloc_function *gim_get_alloc_handler() { - return g_allocfn; + return g_allocfn; } gim_alloca_function *gim_get_alloca_handler() { - return g_allocafn; + return g_allocafn; } - -gim_realloc_function *gim_get_realloc_handler () +gim_realloc_function *gim_get_realloc_handler() { - return g_reallocfn; + return g_reallocfn; } - -gim_free_function *gim_get_free_handler () +gim_free_function *gim_get_free_handler() { - return g_freefn; + return g_freefn; } - -void * gim_alloc(size_t size) +void *gim_alloc(size_t size) { - void * ptr; + void *ptr; if (g_allocfn) { ptr = g_allocfn(size); @@ -93,27 +89,29 @@ void * gim_alloc(size_t size) else { #ifdef GIM_SIMD_MEMORY - ptr = btAlignedAlloc(size,16); + ptr = btAlignedAlloc(size, 16); #else ptr = malloc(size); #endif } - return ptr; + return ptr; } -void * gim_alloca(size_t size) +void *gim_alloca(size_t size) { - if (g_allocafn) return g_allocafn(size); else return gim_alloc(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 *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 *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) @@ -121,15 +119,14 @@ void gim_free(void *ptr) if (!ptr) return; if (g_freefn) { - g_freefn(ptr); + g_freefn(ptr); } else { - #ifdef GIM_SIMD_MEMORY +#ifdef GIM_SIMD_MEMORY btAlignedFree(ptr); - #else +#else free(ptr); - #endif +#endif } } - diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h index e203888a1e2..fffbfa23d8e 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h @@ -32,93 +32,84 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_math.h" #include <string.h> #ifdef PREFETCH -#include <xmmintrin.h> // for prefetch -#define pfval 64 -#define pfval2 128 +#include <xmmintrin.h> // for prefetch +#define pfval 64 +#define pfval2 128 //! Prefetch 64 -#define pf(_x,_i) _mm_prefetch((void *)(_x + _i + pfval), 0) +#define pf(_x, _i) _mm_prefetch((void *)(_x + _i + pfval), 0) //! Prefetch 128 -#define pf2(_x,_i) _mm_prefetch((void *)(_x + _i + pfval2), 0) +#define pf2(_x, _i) _mm_prefetch((void *)(_x + _i + pfval2), 0) #else //! Prefetch 64 -#define pf(_x,_i) +#define pf(_x, _i) //! Prefetch 128 -#define pf2(_x,_i) +#define pf2(_x, _i) #endif - ///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;\ - }\ -}\ - +#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; \ + } \ + } ///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); - +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); ///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); - +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); ///get current memory management functions. -gim_alloc_function *gim_get_alloc_handler (void); +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); - +gim_realloc_function *gim_get_realloc_handler(void); +gim_free_function *gim_get_free_handler(void); ///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_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 +#if defined(_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) +#define GIM_SIMD_MEMORY 1 #endif //! SIMD POINTER INTEGER @@ -126,11 +117,10 @@ void gim_free(void *ptr); //! SIMD INTEGER SIZE #define SIMD_T_SIZE sizeof(SIMD_T) - -inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize) +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; @@ -143,48 +133,45 @@ inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize) 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; + 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); + memcpy(dst, src, copysize); #endif } - - -template<class T> -inline void gim_swap_elements(T* _array,size_t _i,size_t _j) +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) +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)); + 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) +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); + _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 +#endif // GIM_MEMORY_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_pair.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_pair.h new file mode 100644 index 00000000000..56c185a5dc9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_pair.h @@ -0,0 +1,28 @@ +#ifndef GIM_PAIR_H +#define GIM_PAIR_H + + +//! Overlapping pair +struct GIM_PAIR +{ + int m_index1; + int m_index2; + GIM_PAIR() + { + } + + GIM_PAIR(const GIM_PAIR& p) + { + m_index1 = p.m_index1; + m_index2 = p.m_index2; + } + + GIM_PAIR(int index1, int index2) + { + m_index1 = index1; + m_index2 = index2; + } +}; + +#endif //GIM_PAIR_H + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h index c246ef12543..ff7907adca0 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h @@ -40,24 +40,22 @@ email: projectileman@yahoo.com //! Prototype for comparators class less_comparator { - public: - - template<class T,class Z> - inline int operator() ( const T& a, const Z& b ) +public: + template <class T, class Z> + inline int operator()(const T& a, const Z& b) { - return ( a<b?-1:(a>b?1:0)); + 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 ) +public: + template <class T> + inline int operator()(const T& a, const T& b) { - return (int)(a-b); + return (int)(a - b); } }; @@ -65,20 +63,19 @@ class integer_comparator class uint_key_func { public: - template<class T> - inline GUINT operator()( const T& a) + 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) + template <class T> + inline void operator()(T& a, T& b) { a = b; } @@ -88,34 +85,33 @@ public: class memcopy_elements_func { public: - template<class T> - inline void operator()(T& a,T& b) + template <class T> + inline void operator()(T& a, T& b) { - gim_simd_memcpy(&a,&b,sizeof(T)); + 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; - } + 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 + inline bool operator<(const GIM_RSORT_TOKEN& other) const { return (m_key < other.m_key); } - inline bool operator >(const GIM_RSORT_TOKEN& other) const + inline bool operator>(const GIM_RSORT_TOKEN& other) const { return (m_key > other.m_key); } @@ -124,33 +120,28 @@ struct GIM_RSORT_TOKEN //! Prototype for comparators class GIM_RSORT_TOKEN_COMPARATOR { - public: - - inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b ) +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 ) - - +#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) + 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; + GUINT* b1 = b0 + kHist; + GUINT* b2 = b1 + kHist; for (i = 0; i < kHist * 3; ++i) { b0[i] = 0; @@ -159,10 +150,10 @@ inline void gim_radix_sort_rtokens( 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)] ++; + fi = array[i].m_key; + b0[D11_0(fi)]++; + b1[D11_1(fi)]++; + b2[D11_2(fi)]++; } { GUINT sum0 = 0, sum1 = 0, sum2 = 0; @@ -182,7 +173,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; + fi = array[i].m_key; pos = D11_0(fi); pos = ++b0[pos]; sorted[pos].m_key = array[i].m_key; @@ -190,7 +181,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = sorted[i].m_key; + fi = sorted[i].m_key; pos = D11_1(fi); pos = ++b1[pos]; array[pos].m_key = sorted[i].m_key; @@ -198,7 +189,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; + fi = array[i].m_key; pos = D11_2(fi); pos = ++b2[pos]; sorted[pos].m_key = array[i].m_key; @@ -206,9 +197,6 @@ inline void gim_radix_sort_rtokens( } } - - - /// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN /*! *\param array Array of elements to sort @@ -216,21 +204,21 @@ inline void gim_radix_sort_rtokens( *\param element_count element count *\param uintkey_macro Functor which retrieves the integer representation of an array element */ -template<typename T, class GETKEY_CLASS> +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) + 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); + 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 @@ -241,21 +229,21 @@ void gim_radix_sort_array_tokens( \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> +template <typename T, class GETKEY_CLASS, class COPY_CLASS> void gim_radix_sort( - T * array, GUINT element_count, + 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); + 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, @@ -269,20 +257,20 @@ If the element is not found, it returns the nearest upper element position, may \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) +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; + GUINT _j = _end_i + 1; while (_i < _j) { - _k = (_j+_i-1)/2; + _k = (_j + _i - 1) / 2; _comp_result = _comp_macro(_array[_k], _search_key); if (_comp_result == 0) { @@ -291,7 +279,7 @@ bool gim_binary_search_ex( } else if (_comp_result < 0) { - _i = _k+1; + _i = _k + 1; } else { @@ -302,8 +290,6 @@ bool gim_binary_search_ex( 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. @@ -314,26 +300,26 @@ If the element is not found, it returns the nearest upper element position, may \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> +template <class T> bool gim_binary_search( - const T*_array,GUINT _start_i, - GUINT _end_i,const T & _search_key, - GUINT & _result_index) + 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 _j = _end_i + 1; GUINT _k; - while(_i < _j) + while (_i < _j) { - _k = (_j+_i-1)/2; - if(_array[_k]==_search_key) + _k = (_j + _i - 1) / 2; + if (_array[_k] == _search_key) { _result_index = _k; return true; } - else if (_array[_k]<_search_key) + else if (_array[_k] < _search_key) { - _i = _k+1; + _i = _k + 1; } else { @@ -344,27 +330,25 @@ bool gim_binary_search( 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) +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) + while (k <= n / 2) { - int child = 2*k; + int child = 2 * k; - if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0) + if ((child < (int)n) && CompareFunc(pArr[child - 1], pArr[child]) < 0) { child++; } /* pick larger child */ - if (CompareFunc(temp , pArr[child - 1])<0) + if (CompareFunc(temp, pArr[child - 1]) < 0) { /* move child up */ pArr[k - 1] = pArr[child - 1]; @@ -378,29 +362,25 @@ void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc) pArr[k - 1] = temp; } /*downHeap*/ - template <typename T, typename COMP_CLASS> -void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc) +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--) + for (k = n / 2; k > 0; k--) { gim_down_heap(pArr, k, n, CompareFunc); } /* a[1..N] is now a heap */ - while ( n>=2 ) + while (n >= 2) { - gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */ + 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 +#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 index f9727e1d53b..8d83e95da40 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -33,15 +33,13 @@ email: projectileman@yahoo.com #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; + GREAL margin; btVector3 tu_vertices[3]; btVector3 tv_vertices[3]; btVector4 tu_plane; @@ -55,46 +53,47 @@ public: GREAL du0du2; GREAL dv[4]; GREAL dv0dv1; - GREAL dv0dv2; + 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) + 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) + 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; + scale_edge0 = -D2 / (D0 - D2); + scale_edge1 = -D1 / (D2 - D1); + edge_index0 = 2; + edge_index1 = 1; } - else if(D0D2>0.0f) + 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; + 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) + 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; + scale_edge0 = -D0 / (D1 - D0); + scale_edge1 = -D2 / (D0 - D2); + edge_index0 = 0; + edge_index1 = 2; } else { @@ -103,46 +102,44 @@ public: return true; } - //! clip triangle /*! */ SIMD_FORCE_INLINE GUINT clip_triangle( - const btVector4 & tri_plane, - const btVector3 * tripoints, - const btVector3 * srcpoints, - btVector3 * clip_points) + 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); + EDGE_PLANE(tripoints[0], tripoints[1], tri_plane, edgeplane); GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( - edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); + edgeplane, srcpoints[0], srcpoints[1], srcpoints[2], temp_points); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 1 - EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); + EDGE_PLANE(tripoints[1], tripoints[2], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points,clipped_count,temp_points1); + edgeplane, temp_points, clipped_count, temp_points1); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 2 - EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); + EDGE_PLANE(tripoints[2], tripoints[0], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points1,clipped_count,clip_points); + edgeplane, temp_points1, clipped_count, clip_points); return clipped_count; - /*GUINT i0 = (tri_plane.closestAxis()+1)%3; GUINT i1 = (i0+1)%3; // edge 0 @@ -172,13 +169,13 @@ public: } SIMD_FORCE_INLINE void sort_isect( - GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1) + GREAL &isect0, GREAL &isect1, GUINT &e0, GUINT &e1, btVector3 &vec0, btVector3 &vec1) { - if(isect1<isect0) + if (isect1 < isect0) { //swap - GIM_SWAP_NUMBERS(isect0,isect1); - GIM_SWAP_NUMBERS(e0,e1); + GIM_SWAP_NUMBERS(isect0, isect1); + GIM_SWAP_NUMBERS(e0, e1); btVector3 tmp = vec0; vec0 = vec1; vec1 = tmp; @@ -202,53 +199,52 @@ public: // Compute direction of intersection line edge_edge_dir = tu_plane.cross(tv_plane); GREAL Dlen; - VEC_LENGTH(edge_edge_dir,Dlen); + VEC_LENGTH(edge_edge_dir, Dlen); - if(Dlen<0.0001) + if (Dlen < 0.0001) { - return 0; //faces near paralele + return 0; //faces near paralele } - edge_edge_dir*= 1/Dlen;//normalize - + 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; + 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 + 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; + 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 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); + 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)}; + 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); + 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 + 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 (midpoint_u < midpoint_v) { - if(isect_u[1]>=isect_v[1]) // face U casts face 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 + else if (isect_v[0] <= isect_u[0]) // face V casts face U { return 2; } @@ -257,32 +253,31 @@ public: 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 + 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], + 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 + 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 + 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 + 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 + else if (isect_u[0] <= isect_v[0]) // face U casts face V { return 1; } @@ -291,41 +286,39 @@ public: 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 + 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], + 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 + 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 + 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) + 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; @@ -339,103 +332,99 @@ public: //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]); + 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 (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 + if (du[0] < 0) //we need test behind the triangle plane { - distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; - if(distances[0]>margin) return false; //never intersect + 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); + 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 + 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 + distances[0] = (du[0] + du[1] + du[2]) / 3.0f; //centroid - if(distances[0]<0.0f) + 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); + 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] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; } else { - distances[0] = GIM_MIN3(du[0],du[1],du[2]); + 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); + 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]); + 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 (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 + if (dv[0] < 0) //we need test behind the triangle plane { - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; - if(distances[1]>margin) return false; //never intersect + 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); + 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 + 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 + distances[1] = (dv[0] + dv[1] + dv[2]) / 3.0f; //centroid - if(distances[1]<0.0f) + 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); + 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] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; } else { - distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]); } } @@ -448,47 +437,44 @@ public: } else {*/ - bl = 0; - if(distances[0]<distances[1]) bl = 1; + bl = 0; + if (distances[0] < distances[1]) bl = 1; //} - if(bl==2) //edge edge separation + if (bl == 2) //edge edge separation { - if(distances[2]>margin) return false; + 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); + 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 + 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); + 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 + 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); + 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; + if (contacts.m_point_count == 0) return false; return true; } - }; - /*class GIM_TRIANGLE_CALCULATION_CACHE { public: @@ -621,20 +607,13 @@ public: };*/ - - bool GIM_TRIANGLE::collide_triangle_hard_test( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const { - GIM_TRIANGLE_CALCULATION_CACHE calc_cache; + 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); - + 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 index 5b552a1ed51..e6d4bf54708 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -36,26 +36,25 @@ email: projectileman@yahoo.com #include "gim_box_collision.h" #include "gim_clip_polygon.h" - - - +#ifndef MAX_TRI_CLIPPING #define MAX_TRI_CLIPPING 16 +#endif //! 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]; + 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) + 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--) + while (i--) { m_points[i] = other.m_points[i]; } @@ -65,39 +64,36 @@ struct GIM_TRIANGLE_CONTACT_DATA { } - GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other) + 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; + //! 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++) + for (_k = 0; _k < point_count; _k++) { - GREAL _dist = -distance_func(plane,points[_k]) + margin; + GREAL _dist = -distance_func(plane, points[_k]) + margin; - if(_dist>=0.0f) + if (_dist >= 0.0f) { - if(_dist>m_penetration_depth) + if (_dist > m_penetration_depth) { m_penetration_depth = _dist; point_indices[0] = _k; - m_point_count=1; + m_point_count = 1; } - else if((_dist+G_EPSILON)>=m_penetration_depth) + else if ((_dist + G_EPSILON) >= m_penetration_depth) { point_indices[m_point_count] = _k; m_point_count++; @@ -105,88 +101,87 @@ struct GIM_TRIANGLE_CONTACT_DATA } } - for( _k=0;_k<m_point_count;_k++) + 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) + 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 - /*! + 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); + SIMD_FORCE_INLINE void get_triangle_transform(btTransform &triangle_transform) const + { + btMatrix3x3 &matrix = triangle_transform.getBasis(); - btVector3 xaxis = m_vertices[1] - m_vertices[0]; - VEC_NORMALIZE(xaxis); - MAT_SET_X(matrix,xaxis); + btVector3 zaxis; + get_normal(zaxis); + MAT_SET_Z(matrix, zaxis); - //y axis - xaxis = zaxis.cross(xaxis); - MAT_SET_Y(matrix,xaxis); + btVector3 xaxis = m_vertices[1] - m_vertices[0]; + VEC_NORMALIZE(xaxis); + MAT_SET_X(matrix, xaxis); - triangle_transform.setOrigin(m_vertices[0]); - } + //y axis + xaxis = zaxis.cross(xaxis); + MAT_SET_Y(matrix, xaxis); + triangle_transform.setOrigin(m_vertices[0]); + } //! Test triangles by finding separating axis /*! @@ -194,8 +189,8 @@ public: \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; + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const; //! Test boxes before doing hard test /*! @@ -204,16 +199,16 @@ public: \ */ SIMD_FORCE_INLINE bool collide_triangle( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const + 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; + 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); + return collide_triangle_hard_test(other, contact_data); } /*! @@ -245,43 +240,43 @@ 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 + 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 _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) + 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]; + 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]; + 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) + if (u < -G_EPSILON) { return false; } - else if(v<-G_EPSILON) + else if (v < -G_EPSILON) { return false; } else { btScalar sumuv; - sumuv = u+v; - if(sumuv<-G_EPSILON) + sumuv = u + v; + if (sumuv < -G_EPSILON) { return false; } - else if(sumuv-1.0f>G_EPSILON) + else if (sumuv - 1.0f > G_EPSILON) { return false; } @@ -293,50 +288,49 @@ if 0.0<= u+v <=1.0 then they are inside of triangle /*! 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 + 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(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(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 + 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) + 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); + 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; + 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 + if (res == 2) //invert normal { - triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); + triangle_normal.setValue(-faceplane[0], -faceplane[1], -faceplane[2]); } else { - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); } VEC_NORMALIZE(triangle_normal); @@ -344,36 +338,31 @@ if 0.0<= u+v <=1.0 then they are inside of triangle 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) + 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); + 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; + 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; + if (!is_point_inside(pout, faceplane)) return false; - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); VEC_NORMALIZE(triangle_normal); return true; } - }; - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h index 9eb880b8dfc..3c82133037e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h @@ -16,163 +16,153 @@ subject to the following restrictions: #ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H #define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H -#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision... +#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision... #include "btGjkEpa3.h" #include "btGjkCollisionDescription.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +template <typename btConvexTemplate> +bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b, + const btGjkCollisionDescription& colDesc, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB) +{ + (void)v; + // const btScalar radialmargin(btScalar(0.)); + btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input? + btGjkEpaSolver3::sResults results; + if (btGjkEpaSolver3_Penetration(a, b, guessVector, results)) -template <typename btConvexTemplate> -bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b, - const btGjkCollisionDescription& colDesc, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB) -{ - (void)v; - - // const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input? - - btGjkEpaSolver3::sResults results; - - - if(btGjkEpaSolver3_Penetration(a,b,guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } else - { - if(btGjkEpaSolver3_Distance(a,b,guessVector,results)) - { - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return false; - } - } - return false; + { + // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return true; + } + else + { + if (btGjkEpaSolver3_Distance(a, b, guessVector, results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } + } + return false; } template <typename btConvexTemplate, typename btGjkDistanceTemplate> -int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo) +int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo) { - - bool m_catchDegeneracies = true; - btScalar m_cachedSeparatingDistance = 0.f; - - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); - - btVector3 pointOnA,pointOnB; - btTransform localTransA = a.getWorldTransform(); - btTransform localTransB = b.getWorldTransform(); - - btScalar marginA = a.getMargin(); - btScalar marginB = b.getMargin(); - - int m_curIter = 0; - int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN? - btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir; - - bool isValid = false; - bool checkSimplex = false; - bool checkPenetration = true; - int m_degenerateSimplex = 0; - - int m_lastUsedMethod = -1; - - { - btScalar squaredDistance = BT_LARGE_FLOAT; - btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - - simplexSolver.reset(); - - for ( ; ; ) - //while (true) - { - - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis(); - - btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA); - btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - - - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared)) - { - m_degenerateSimplex = 10; - checkSimplex=true; - //checkPenetration = false; - break; - } - - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (simplexSolver.inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } else - { - m_degenerateSimplex = 11; - } - checkSimplex = true; - break; - } - - //add current vertex to simplex - simplexSolver.addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; - - //calculate the closest point to the origin (update vector v) - if (!simplexSolver.closest(newCachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2) - { - m_cachedSeparatingAxis = newCachedSeparatingAxis; - m_degenerateSimplex = 6; - checkSimplex = true; - break; - } - - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = newCachedSeparatingAxis.length2(); + bool m_catchDegeneracies = true; + btScalar m_cachedSeparatingDistance = 0.f; + + btScalar distance = btScalar(0.); + btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.)); + + btVector3 pointOnA, pointOnB; + btTransform localTransA = a.getWorldTransform(); + btTransform localTransB = b.getWorldTransform(); + + btScalar marginA = a.getMargin(); + btScalar marginB = b.getMargin(); + + int m_curIter = 0; + int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN? + btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir; + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + int m_degenerateSimplex = 0; + + int m_lastUsedMethod = -1; + + { + btScalar squaredDistance = BT_LARGE_FLOAT; + btScalar delta = btScalar(0.); + + btScalar margin = marginA + marginB; + + simplexSolver.reset(); + + for (;;) + //while (true) + { + btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis(); + btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis(); + + btVector3 pInA = a.getLocalSupportWithoutMargin(separatingAxisInA); + btVector3 qInB = b.getLocalSupportWithoutMargin(separatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared)) + { + m_degenerateSimplex = 10; + checkSimplex = true; + //checkPenetration = false; + break; + } + + //exit 0: the new point is already in the simplex, or we didn't come any closer + if (simplexSolver.inSimplex(w)) + { + m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; + + if (f0 <= f1) + { + if (f0 <= btScalar(0.)) + { + m_degenerateSimplex = 2; + } + else + { + m_degenerateSimplex = 11; + } + checkSimplex = true; + break; + } + + //add current vertex to simplex + simplexSolver.addVertex(w, pWorld, qWorld); + btVector3 newCachedSeparatingAxis; + + //calculate the closest point to the origin (update vector v) + if (!simplexSolver.closest(newCachedSeparatingAxis)) + { + m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2) + { + m_cachedSeparatingAxis = newCachedSeparatingAxis; + m_degenerateSimplex = 6; + checkSimplex = true; + break; + } + + btScalar previousSquaredDistance = squaredDistance; + squaredDistance = newCachedSeparatingAxis.length2(); #if 0 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo if (squaredDistance>previousSquaredDistance) @@ -182,188 +172,183 @@ int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate checkSimplex = false; break; } -#endif // - - - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); - - //are we getting any closer ? - if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) - { - // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - checkSimplex = true; - m_degenerateSimplex = 12; - - break; - } - - m_cachedSeparatingAxis = newCachedSeparatingAxis; - - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { -#if defined(DEBUG) || defined (_DEBUG) - - printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance); +#endif // + + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + + //are we getting any closer ? + if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) + { + // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + checkSimplex = true; + m_degenerateSimplex = 12; + + break; + } + + m_cachedSeparatingAxis = newCachedSeparatingAxis; + + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (m_curIter++ > gGjkMaxIter) + { +#if defined(DEBUG) || defined(_DEBUG) + + printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance); #endif - - break; - - } - - - bool check = (!simplexSolver.fullSimplex()); - //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); - - if (!check) - { - //do we need this backup_closest here ? - // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - m_degenerateSimplex = 13; - break; - } - } - - if (checkSimplex) - { - simplexSolver.compute_points(pointOnA, pointOnB); - normalInB = m_cachedSeparatingAxis; - - btScalar lenSqr =m_cachedSeparatingAxis.length2(); - - //valid normal - if (lenSqr < 0.0001) - { - m_degenerateSimplex = 5; - } - if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - normalInB *= rlen; //normalize - - btScalar s = btSqrt(squaredDistance); - - btAssert(s > btScalar(0.0)); - pointOnA -= m_cachedSeparatingAxis * (marginA / s); - pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((btScalar(1.)/rlen) - margin); - isValid = true; - - m_lastUsedMethod = 1; - } else - { - m_lastUsedMethod = 2; - } - } - - bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_degenerateSimplex && ((distance+margin) < 0.01)); - - //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) - { - //penetration case - - //if there is no way to handle penetrations, bail out - - // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - m_cachedSeparatingAxis.setZero(); - - bool isValid2 = btGjkEpaCalcPenDepth(a,b, - colDesc, - m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB); - - if (isValid2) - { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB = m_cachedSeparatingAxis; - lenSqr = m_cachedSeparatingAxis.length2(); - } - - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - - isValid = true; - m_lastUsedMethod = 3; - } else - { - m_lastUsedMethod = 8; - } - } else - { - m_lastUsedMethod = 9; - } - } else - - { - ///this is another degenerate case, where the initial GJK calculation reports a degenerate case - ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) - ///reports a valid positive distance. Use the results of the second GJK instead of failing. - ///thanks to Jacob.Langford for the reproduction case - ///http://code.google.com/p/bullet/issues/detail?id=250 - - - if (m_cachedSeparatingAxis.length2() > btScalar(0.)) - { - btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; - //only replace valid distances when the distance is less - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - pointOnA -= m_cachedSeparatingAxis * marginA ; - pointOnB += m_cachedSeparatingAxis * marginB ; - normalInB = m_cachedSeparatingAxis; - normalInB.normalize(); - - isValid = true; - m_lastUsedMethod = 6; - } else - { - m_lastUsedMethod = 5; - } - } - } - } - } - - - - if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared))) - { - - m_cachedSeparatingAxis = normalInB; - m_cachedSeparatingDistance = distance; - distInfo->m_distance = distance; - distInfo->m_normalBtoA = normalInB; - distInfo->m_pointOnB = pointOnB; - distInfo->m_pointOnA = pointOnB+normalInB*distance; - return 0; - } - return -m_lastUsedMethod; -} + break; + } + + bool check = (!simplexSolver.fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + + if (!check) + { + //do we need this backup_closest here ? + // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; + break; + } + } + + if (checkSimplex) + { + simplexSolver.compute_points(pointOnA, pointOnB); + normalInB = m_cachedSeparatingAxis; + + btScalar lenSqr = m_cachedSeparatingAxis.length2(); + + //valid normal + if (lenSqr < 0.0001) + { + m_degenerateSimplex = 5; + } + if (lenSqr > SIMD_EPSILON * SIMD_EPSILON) + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); + normalInB *= rlen; //normalize + + btScalar s = btSqrt(squaredDistance); + + btAssert(s > btScalar(0.0)); + pointOnA -= m_cachedSeparatingAxis * (marginA / s); + pointOnB += m_cachedSeparatingAxis * (marginB / s); + distance = ((btScalar(1.) / rlen) - margin); + isValid = true; + + m_lastUsedMethod = 1; + } + else + { + m_lastUsedMethod = 2; + } + } + + bool catchDegeneratePenetrationCase = + (m_catchDegeneracies && m_degenerateSimplex && ((distance + margin) < 0.01)); + + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) + { + //penetration case + //if there is no way to handle penetrations, bail out + // Penetration depth case. + btVector3 tmpPointOnA, tmpPointOnB; + + m_cachedSeparatingAxis.setZero(); + + bool isValid2 = btGjkEpaCalcPenDepth(a, b, + colDesc, + m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB); + + if (isValid2) + { + btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) + { + tmpNormalInB = m_cachedSeparatingAxis; + lenSqr = m_cachedSeparatingAxis.length2(); + } + + if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON)) + { + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length(); + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + + isValid = true; + m_lastUsedMethod = 3; + } + else + { + m_lastUsedMethod = 8; + } + } + else + { + m_lastUsedMethod = 9; + } + } + else + + { + ///this is another degenerate case, where the initial GJK calculation reports a degenerate case + ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) + ///reports a valid positive distance. Use the results of the second GJK instead of failing. + ///thanks to Jacob.Langford for the reproduction case + ///http://code.google.com/p/bullet/issues/detail?id=250 + + if (m_cachedSeparatingAxis.length2() > btScalar(0.)) + { + btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin; + //only replace valid distances when the distance is less + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + pointOnA -= m_cachedSeparatingAxis * marginA; + pointOnB += m_cachedSeparatingAxis * marginB; + normalInB = m_cachedSeparatingAxis; + normalInB.normalize(); + + isValid = true; + m_lastUsedMethod = 6; + } + else + { + m_lastUsedMethod = 5; + } + } + } + } + } + + if (isValid && ((distance < 0) || (distance * distance < colDesc.m_maximumDistanceSquared))) + { + m_cachedSeparatingAxis = normalInB; + m_cachedSeparatingDistance = distance; + distInfo->m_distance = distance; + distInfo->m_normalBtoA = normalInB; + distInfo->m_pointOnB = pointOnB; + distInfo->m_pointOnA = pointOnB + normalInB * distance; + return 0; + } + return -m_lastUsedMethod; +} -#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H +#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 940282f5762..38df8d4808e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btContinuousConvexCollision.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" @@ -24,59 +23,60 @@ subject to the following restrictions: #include "btPointCollector.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - - -btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_simplexSolver(simplexSolver), -m_penetrationDepthSolver(penetrationDepthSolver), -m_convexA(convexA),m_convexB1(convexB),m_planeShape(0) +btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) + : m_simplexSolver(simplexSolver), + m_penetrationDepthSolver(penetrationDepthSolver), + m_convexA(convexA), + m_convexB1(convexB), + m_planeShape(0) { } - -btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape* convexA,const btStaticPlaneShape* plane) -:m_simplexSolver(0), -m_penetrationDepthSolver(0), -m_convexA(convexA),m_convexB1(0),m_planeShape(plane) +btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btStaticPlaneShape* plane) + : m_simplexSolver(0), + m_penetrationDepthSolver(0), + m_convexA(convexA), + m_convexB1(0), + m_planeShape(plane) { } - /// 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 64 -void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector) +void btContinuousConvexCollision::computeClosestPoints(const btTransform& transA, const btTransform& transB, btPointCollector& pointCollector) { if (m_convexB1) { m_simplexSolver->reset(); - btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA, m_convexB1, m_convexA->getShapeType(), m_convexB1->getShapeType(), m_convexA->getMargin(), m_convexB1->getMargin(), m_simplexSolver, m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; input.m_transformA = transA; input.m_transformB = transB; - gjk.getClosestPoints(input,pointCollector,0); - } else + gjk.getClosestPoints(input, pointCollector, 0); + } + else { //convex versus plane const btConvexShape* convexShape = m_convexA; const btStaticPlaneShape* planeShape = m_planeShape; - + const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - + btTransform convexWorldTransform = transA; btTransform convexInPlaneTrans; - convexInPlaneTrans= transB.inverse() * convexWorldTransform; + convexInPlaneTrans = transB.inverse() * convexWorldTransform; btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * transB; - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + planeInConvex = convexWorldTransform.inverse() * transB; + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected; btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal; @@ -87,41 +87,33 @@ void btContinuousConvexCollision::computeClosestPoints( const btTransform& trans } } -bool btContinuousConvexCollision::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) +bool btContinuousConvexCollision::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) { - - /// compute linear and angular velocity for this interval, to interpolate - btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA); - btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB); - + btVector3 linVelA, angVelA, linVelB, angVelB; + btTransformUtil::calculateVelocity(fromA, toA, btScalar(1.), linVelA, angVelA); + btTransformUtil::calculateVelocity(fromB, toB, btScalar(1.), linVelB, angVelB); btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); - btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f; + btScalar boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f; btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; - btVector3 relLinVel = (linVelB-linVelA); - - btScalar relLinVelocLength = (linVelB-linVelA).length(); - - if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) - return false; + btVector3 relLinVel = (linVelB - linVelA); + btScalar relLinVelocLength = (linVelB - linVelA).length(); + if ((relLinVelocLength + maxAngularProjectedVelocity) == 0.f) + return false; btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); - - int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); bool hasResult = false; btVector3 c; @@ -131,15 +123,13 @@ bool btContinuousConvexCollision::calcTimeOfImpact( int numIter = 0; //first solution, using GJK - btScalar radius = 0.001f; -// result.drawCoordSystem(sphereTr); + // result.drawCoordSystem(sphereTr); - btPointCollector pointCollector1; + btPointCollector pointCollector1; { - - computeClosestPoints(fromA,fromB,pointCollector1); + computeClosestPoints(fromA, fromB, pointCollector1); hasResult = pointCollector1.m_hasResult; c = pointCollector1.m_pointInWorld; @@ -151,7 +141,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( dist = pointCollector1.m_distance + result.m_allowedPenetration; n = pointCollector1.m_normalOnBInWorld; btScalar projectedLinearVelocity = relLinVel.dot(n); - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON) return false; //not close enough @@ -159,77 +149,69 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { if (result.m_debugDrawer) { - result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1)); + result.m_debugDrawer->drawSphere(c, 0.2f, btVector3(1, 1, 1)); } btScalar dLambda = btScalar(0.); projectedLinearVelocity = relLinVel.dot(n); - //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON) return false; - - dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); - - - lambda = lambda + dLambda; + dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); - if (lambda > btScalar(1.)) - return false; + lambda += dLambda; - if (lambda < btScalar(0.)) + if (lambda > btScalar(1.) || lambda < btScalar(0.)) return false; - //todo: next check with relative epsilon if (lambda <= lastLambda) { return false; //n.setValue(0,0,0); - break; + //break; } lastLambda = lambda; - - //interpolate to next lambda - btTransform interpolatedTransA,interpolatedTransB,relativeTrans; + btTransform interpolatedTransA, interpolatedTransB, relativeTrans; - btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); - btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); + btTransformUtil::integrateTransform(fromA, linVelA, angVelA, lambda, interpolatedTransA); + btTransformUtil::integrateTransform(fromB, linVelB, angVelB, lambda, interpolatedTransB); relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); if (result.m_debugDrawer) { - result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0)); + result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0)); } - result.DebugDraw( lambda ); + result.DebugDraw(lambda); - btPointCollector pointCollector; - computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector); + btPointCollector pointCollector; + computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector); if (pointCollector.m_hasResult) { - dist = pointCollector.m_distance+result.m_allowedPenetration; - c = pointCollector.m_pointInWorld; + dist = pointCollector.m_distance + result.m_allowedPenetration; + c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; - } else + } + else { result.reportFailure(-1, numIter); return false; } numIter++; - if (numIter > maxIter) + if (numIter > MAX_ITERATIONS) { result.reportFailure(-2, numIter); return false; } } - + result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; @@ -237,6 +219,4 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } return false; - } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h index bdc0572f75a..67b2205c366 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H #define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H @@ -25,35 +24,30 @@ class btStaticPlaneShape; /// btContinuousConvexCollision implements angular and linear time of impact for convex objects. /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). -/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. +/// Algorithm operates in worldspace, in order to keep in between motion globally consistent. /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops class btContinuousConvexCollision : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; - const btConvexShape* m_convexA; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + const btConvexShape* m_convexA; //second object is either a convex or a plane (code sharing) - const btConvexShape* m_convexB1; - const btStaticPlaneShape* m_planeShape; + const btConvexShape* m_convexB1; + const btStaticPlaneShape* m_planeShape; - void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector); + void computeClosestPoints(const btTransform& transA, const btTransform& transB, struct btPointCollector& pointCollector); public: + btContinuousConvexCollision(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); - btContinuousConvexCollision (const btConvexShape* shapeA,const btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - - btContinuousConvexCollision(const btConvexShape* shapeA,const btStaticPlaneShape* plane ); - - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + btContinuousConvexCollision(const btConvexShape* shapeA, const btStaticPlaneShape* plane); + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; - -#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H - +#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index bfd79d03beb..77b19be599f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONVEX_CAST_H #define BT_CONVEX_CAST_H @@ -23,51 +22,69 @@ subject to the following restrictions: class btMinkowskiSumShape; #include "LinearMath/btIDebugDraw.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_CONVEX_CAST_ITERATIONS 64 +#define MAX_CONVEX_CAST_EPSILON (SIMD_EPSILON * 10) +#else +#define MAX_CONVEX_CAST_ITERATIONS 32 +#define MAX_CONVEX_CAST_EPSILON btScalar(0.0001) +#endif +///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. +///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 +//will need to digg deeper to make the algorithm more robust +//since, a large epsilon can cause an early termination with false +//positive results (ray intersections that shouldn't be there) + /// btConvexCast is an interface for Casting class btConvexCast { public: - - virtual ~btConvexCast(); ///RayResult stores the closest result /// alternatively, add a callback method to decide about closest/all results - struct CastResult + struct CastResult { //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0; - - virtual void DebugDraw(btScalar fraction) {(void)fraction;} - virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} - virtual void reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;} + + virtual void DebugDraw(btScalar fraction) { (void)fraction; } + virtual void drawCoordSystem(const btTransform& trans) { (void)trans; } + virtual void reportFailure(int errNo, int numIterations) + { + (void)errNo; + (void)numIterations; + } CastResult() - :m_fraction(btScalar(BT_LARGE_FLOAT)), - m_debugDrawer(0), - m_allowedPenetration(btScalar(0)) + : m_fraction(btScalar(BT_LARGE_FLOAT)), + m_debugDrawer(0), + m_allowedPenetration(btScalar(0)), + m_subSimplexCastMaxIterations(MAX_CONVEX_CAST_ITERATIONS), + m_subSimplexCastEpsilon(MAX_CONVEX_CAST_EPSILON) { } + virtual ~CastResult(){}; - virtual ~CastResult() {}; - - btTransform m_hitTransformA; - btTransform m_hitTransformB; - btVector3 m_normal; - btVector3 m_hitPoint; - btScalar m_fraction; //input and output + btTransform m_hitTransformA; + btTransform m_hitTransformB; + btVector3 m_normal; + btVector3 m_hitPoint; + btScalar m_fraction; //input and output btIDebugDraw* m_debugDrawer; - btScalar m_allowedPenetration; + btScalar m_allowedPenetration; + + int m_subSimplexCastMaxIterations; + btScalar m_subSimplexCastEpsilon; }; - /// cast a convex against another convex object - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) = 0; + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) = 0; }; -#endif //BT_CONVEX_CAST_H +#endif //BT_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 29620abffb9..65c9df9340b 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONVEX_PENETRATION_DEPTH_H #define BT_CONVEX_PENETRATION_DEPTH_H @@ -25,16 +24,12 @@ class btTransform; ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. class btConvexPenetrationDepthSolver { -public: - - virtual ~btConvexPenetrationDepthSolver() {}; - virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw) = 0; - - +public: + virtual ~btConvexPenetrationDepthSolver(){}; + virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw) = 0; }; -#endif //BT_CONVEX_PENETRATION_DEPTH_H - +#endif //BT_CONVEX_PENETRATION_DEPTH_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index 46ce1ab75e7..d1bbb1a46ee 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H #define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H @@ -27,62 +26,60 @@ subject to the following restrictions: /// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB struct btDiscreteCollisionDetectorInterface { - struct Result { - - virtual ~Result(){} + virtual ~Result() {} ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner - virtual void setShapeIdentifiersA(int partId0,int index0)=0; - virtual void setShapeIdentifiersB(int partId1,int index1)=0; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; + virtual void setShapeIdentifiersA(int partId0, int index0) = 0; + virtual void setShapeIdentifiersB(int partId1, int index1) = 0; + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) = 0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) + : m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) { } btTransform m_transformA; btTransform m_transformB; - btScalar m_maximumDistanceSquared; + btScalar m_maximumDistanceSquared; }; - virtual ~btDiscreteCollisionDetectorInterface() {}; + virtual ~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,bool swapResults=false) = 0; - + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false) = 0; }; struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result { - btVector3 m_normalOnSurfaceB; - btVector3 m_closestPointInB; - btScalar m_distance; //negative means penetration ! + btVector3 m_normalOnSurfaceB; + btVector3 m_closestPointInB; + btScalar m_distance; //negative means penetration ! - btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) - { +protected: + btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) + { + } - } - virtual ~btStorageResult() {}; +public: + virtual ~btStorageResult(){}; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) + { + if (depth < m_distance) { - if (depth < m_distance) - { - m_normalOnSurfaceB = normalOnBInWorld; - m_closestPointInB = pointInWorld; - m_distance = depth; - } + m_normalOnSurfaceB = normalOnBInWorld; + m_closestPointInB = pointInWorld; + m_distance = depth; } + } }; -#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H - +#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h index 0b49b0ecc65..c9fd84bebfa 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GJK_COLLISION_DESCRIPTION_H #define GJK_COLLISION_DESCRIPTION_H @@ -21,21 +20,20 @@ subject to the following restrictions: struct btGjkCollisionDescription { - btVector3 m_firstDir; - int m_maxGjkIterations; - btScalar m_maximumDistanceSquared; - btScalar m_gjkRelError2; - btGjkCollisionDescription() - :m_firstDir(0,1,0), - m_maxGjkIterations(1000), - m_maximumDistanceSquared(1e30f), - m_gjkRelError2(1.0e-6) - { - } - virtual ~btGjkCollisionDescription() - { - } + btVector3 m_firstDir; + int m_maxGjkIterations; + btScalar m_maximumDistanceSquared; + btScalar m_gjkRelError2; + btGjkCollisionDescription() + : m_firstDir(0, 1, 0), + m_maxGjkIterations(1000), + m_maximumDistanceSquared(1e30f), + m_gjkRelError2(1.0e-6) + { + } + virtual ~btGjkCollisionDescription() + { + } }; -#endif //GJK_COLLISION_DESCRIPTION_H - +#endif //GJK_COLLISION_DESCRIPTION_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index bef697a0a11..9d61e75dac8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btGjkConvexCast.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "btGjkPairDetector.h" @@ -27,41 +25,39 @@ subject to the following restrictions: #define MAX_ITERATIONS 32 #endif -btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) -:m_simplexSolver(simplexSolver), -m_convexA(convexA), -m_convexB(convexB) +btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver) + : m_simplexSolver(simplexSolver), + m_convexA(convexA), + m_convexB(convexB) { } -bool btGjkConvexCast::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) +bool btGjkConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) { - - 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(); + 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); + btVector3 v(1, 0, 0); int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); bool hasResult = false; btVector3 c; - btVector3 r = (linVelA-linVelB); + btVector3 r = (linVelA - linVelB); btScalar lastLambda = lambda; //btScalar epsilon = btScalar(0.001); @@ -69,17 +65,14 @@ bool btGjkConvexCast::calcTimeOfImpact( int numIter = 0; //first solution, using GJK - btTransform identityTrans; identityTrans.setIdentity(); + // result.drawCoordSystem(sphereTr); -// result.drawCoordSystem(sphereTr); - - btPointCollector pointCollector; + btPointCollector pointCollector; - - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver, 0); //m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD @@ -87,7 +80,7 @@ bool btGjkConvexCast::calcTimeOfImpact( input.m_transformA = fromA; input.m_transformB = fromB; - gjk.getClosestPoints(input,pointCollector,0); + gjk.getClosestPoints(input, pointCollector, 0); hasResult = pointCollector.m_hasResult; c = pointCollector.m_pointInWorld; @@ -98,20 +91,18 @@ bool btGjkConvexCast::calcTimeOfImpact( dist = pointCollector.m_distance; n = pointCollector.m_normalOnBInWorld; - - //not close enough while (dist > radius) { numIter++; if (numIter > maxIter) { - return false; //todo: report a failure + return false; //todo: report a failure } btScalar dLambda = btScalar(0.); btScalar projectedLinearVelocity = r.dot(n); - + dLambda = dist / (projectedLinearVelocity); lambda = lambda - dLambda; @@ -132,35 +123,35 @@ bool btGjkConvexCast::calcTimeOfImpact( lastLambda = lambda; //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); + 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.)) { result.m_fraction = lastLambda; n = pointCollector.m_normalOnBInWorld; - result.m_normal=n; + result.m_normal = n; result.m_hitPoint = pointCollector.m_pointInWorld; return true; } - c = pointCollector.m_pointInWorld; + c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; dist = pointCollector.m_distance; - } else + } + else { //?? return false; } - } //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) + if (n.dot(r) >= -result.m_allowedPenetration) return false; result.m_fraction = lambda; @@ -170,7 +161,4 @@ bool btGjkConvexCast::calcTimeOfImpact( } return false; - - } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index 6a42ee63b03..ef5979173e9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_GJK_CONVEX_CAST_H #define BT_GJK_CONVEX_CAST_H @@ -29,22 +27,20 @@ class btMinkowskiSumShape; ///GjkConvexCast performs a raycast on a convex object using support mapping. class btGjkConvexCast : public btConvexCast { - btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_convexA; - const btConvexShape* m_convexB; + btSimplexSolverInterface* m_simplexSolver; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - - btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver); + btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver); /// cast a convex against another convex object - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; -#endif //BT_GJK_CONVEX_CAST_H +#endif //BT_GJK_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 3268f06c2f9..7d53f8624a8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -26,996 +26,1069 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "btGjkEpa2.h" -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf +#if defined(DEBUG) || defined(_DEBUG) +#include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif namespace gjkepa2_impl { +// Config - // Config +/* GJK */ +#define GJK_MAX_ITERATIONS 128 - /* 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) +#ifdef BT_USE_DOUBLE_PRECISION +#define GJK_ACCURACY ((btScalar)1e-12) +#define GJK_MIN_DISTANCE ((btScalar)1e-12) +#define GJK_DUPLICATED_EPS ((btScalar)1e-12) +#else +#define GJK_ACCURACY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#endif //BT_USE_DOUBLE_PRECISION +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - // Shorthands - typedef unsigned int U; - typedef unsigned char U1; +/* EPA */ +#define EPA_MAX_VERTICES 128 +#define EPA_MAX_ITERATIONS 255 - // MinkowskiDiff - struct MinkowskiDiff - { - const btConvexShape* m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; -#ifdef __SPU__ - bool m_enableMargin; +#ifdef BT_USE_DOUBLE_PRECISION +#define EPA_ACCURACY ((btScalar)1e-12) +#define EPA_PLANE_EPS ((btScalar)1e-14) +#define EPA_INSIDE_EPS ((btScalar)1e-9) #else - btVector3 (btConvexShape::*Ls)(const btVector3&) const; -#endif//__SPU__ - +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) +#endif - MinkowskiDiff() - { +#define EPA_FALLBACK (10 * EPA_ACCURACY) +#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2) - } +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff +{ + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; #ifdef __SPU__ - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - if (m_enableMargin) - { - return m_shapes[0]->localGetSupportVertexNonVirtual(d); - } else - { - return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); - } - } - inline btVector3 Support1(const btVector3& d) const - { - if (m_enableMargin) - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); - } else - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); - } - } + bool m_enableMargin; #else - void EnableMargin(bool enable) - { - if(enable) - Ls=&btConvexShape::localGetSupportVertexNonVirtual; - else - Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; - } - inline btVector3 Support0(const btVector3& d) const + btVector3 (btConvexShape::*Ls)(const btVector3&) const; +#endif //__SPU__ + + MinkowskiDiff() + { + } +#ifdef __SPU__ + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + if (m_enableMargin) { - return(((m_shapes[0])->*(Ls))(d)); + return m_shapes[0]->localGetSupportVertexNonVirtual(d); } - inline btVector3 Support1(const btVector3& d) const + else { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); } -#endif //__SPU__ - - inline btVector3 Support(const btVector3& d) const + } + inline btVector3 Support1(const btVector3& d) const + { + if (m_enableMargin) { - return(Support0(d)-Support1(-d)); + return m_toshape0 * (m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1 * d)); } - btVector3 Support(const btVector3& d,U index) const + else { - if(index) - return(Support1(d)); - else - return(Support0(d)); + return m_toshape0 * (m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1 * d)); } - }; + } +#else + void EnableMargin(bool enable) + { + if (enable) + Ls = &btConvexShape::localGetSupportVertexNonVirtual; + else + Ls = &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; + } + 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)); + } +#endif //__SPU__ - typedef MinkowskiDiff tShape; + 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 +// GJK +struct GJK +{ + /* Types */ + struct sSV { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex + btVector3 d, w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + struct eStatus + { + enum _ { - 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(); + 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; } - void Initialize() + /* 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) { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; + if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS) + { + found = true; + break; + } } - eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + 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 = btDot(m_ray, w) / rl; + alpha = btMax(omega, alpha); + if (((rl - alpha) - (GJK_ACCURACY * rl)) <= 0) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask = 0; + switch (cs.rank) { - 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) + 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)) { - if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) - { found=true;break; } - } - if(found) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w * weights[i]; } else - {/* Update lastw */ - lastw[clastw=(clastw+1)&3]=w; - } - /* Check for termination */ - const btScalar omega=btDot(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_free[m_nfree++] = cs.c[i]; } - m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; - } while(m_status==eStatus::Valid); - m_simplex=&m_simplices[m_current]; - switch(m_status) + } + 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; + default: + { + } + } + return (m_status); + } + bool EncloseOrigin() + { + switch (m_simplex->rank) + { + case 1: + { + for (U i = 0; i < 3; ++i) { - case eStatus::Valid: m_distance=m_ray.length();break; - case eStatus::Inside: m_distance=0;break; - default: - { - } - } - return(m_status); + 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); + } } - bool EncloseOrigin() + break; + case 2: { - switch(m_simplex->rank) + const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w; + for (U i = 0; i < 3; ++i) { - case 1: + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + const btVector3 p = btCross(d, axis); + if (p.length2() > 0) { - 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=btCross(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); - } - } + 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=btCross(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 + break; + case 3: { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); + const btVector3 n = btCross(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); + } } - void removevertice(sSimplex& simplex) + break; + case 4: { - m_free[m_nfree++]=simplex.c[--simplex.rank]; + 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); } - void appendvertice(sSimplex& simplex,const btVector3& v) + 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 ? -btDot(a, d) / l : 0); + if (t >= 1) { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); + w[0] = 0; + w[1] = 1; + m = 2; + return (b.length2()); } - static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + else if (t <= 0) { - 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()); + w[0] = 1; + w[1] = 0; + m = 1; + return (a.length2()); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) + else { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-btDot(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); + w[0] = 1 - (w[1] = t); + m = 3; + return ((a + d * t).length2()); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) + } + 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 = btCross(dl[0], dl[1]); + const btScalar l = n.length2(); + if (l > GJK_SIMPLEX3_EPS) + { + btScalar mindist = -1; + btScalar subw[2] = {0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) { - 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=btCross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) + if (btDot(*vt[i], btCross(dl[i], n)) > 0) { - btScalar mindist=-1; - btScalar subw[2]={0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) + const U j = imd3[i]; + const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm)); + if ((mindist < 0) || (subd < mindist)) { - if(btDot(*vt[i],btCross(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; - } - } + 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=btDot(a,n); - const btScalar s=btSqrt(l); - const btVector3 p=n*(d/l); - mindist = p.length2(); - m = 7; - w[0] = (btCross(dl[1],b-p)).length()/s; - w[1] = (btCross(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) + if (mindist < 0) { - 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*btDot(a,btCross(b-c,a-b)))<=0; - if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) + const btScalar d = btDot(a, n); + const btScalar s = btSqrt(l); + const btVector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1], b - p)).length() / s; + w[1] = (btCross(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 * btDot(a, btCross(b - c, a - b))) <= 0; + if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS)) + { + btScalar mindist = -1; + btScalar subw[3] = {0.f, 0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + const U j = imd3[i]; + const btScalar s = vl * btDot(d, btCross(dl[i], dl[j])); + if (s > 0) { - btScalar mindist=-1; - btScalar subw[3]={0.f,0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*btDot(d,btCross(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) + const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm); + if ((mindist < 0) || (subd < mindist)) { - 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]); + 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]; } - return(mindist); } - return(-1); } - }; + 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 +// EPA +struct EPA +{ + /* Types */ + typedef GJK::sSV sSV; + struct sFace { - /* Types */ - typedef GJK::sSV sSV; - struct sFace - { - btVector3 n; - btScalar d; - 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 + btVector3 n; + btScalar d; + 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 _ { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(0),ff(0),nf(0) {} - }; - struct eStatus { enum _ { Valid, Touching, Degenerated, NonConvex, - InvalidHull, + 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(); - } + 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; + } - 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) + 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) { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; + sFace* f = m_hull.root; + remove(m_hull, f); + append(m_stock, f); } - static inline void remove(sList& list,sFace* face) + 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) { - 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; + btSwap(simplex.c[0], simplex.c[1]); + btSwap(simplex.p[0], simplex.p[1]); } - - - void Initialize() + /* 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) { - 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) + 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) { - 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) + if (m_nextsv < EPA_MAX_VERTICES) { - 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) + sHorizon horizon; + sSV* w = &m_sv_store[m_nextsv++]; + bool valid = true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n, *w); + const btScalar wdist = btDot(best->n, w->w) - best->d; + if (wdist > EPA_ACCURACY) { - 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=btDot(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(); - outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } + 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(); + outer = *best; + } + else + { + m_status = eStatus::InvalidHull; + break; + } + } + else + { + m_status = eStatus::AccuraryReached; + 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] = btCross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = btCross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = btCross( 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); - } - bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist) - { - const btVector3 ba = b->w - a->w; - const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane - const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - - if(a_dot_nab < 0) - { - // Outside of edge a->b - - const btScalar ba_l2 = ba.length2(); - const btScalar a_dot_ba = btDot(a->w, ba); - const btScalar b_dot_ba = btDot(b->w, ba); - - if(a_dot_ba > 0) - { - // Pick distance vertex a - dist = a->w.length(); - } - else if(b_dot_ba < 0) - { - // Pick distance vertex b - dist = b->w.length(); } else { - // Pick distance to edge a->b - const btScalar a_dot_b = btDot(a->w, b->w); - dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + m_status = eStatus::OutOfVertices; + break; } - - return true; } + 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] = btCross(outer.c[1]->w - projection, + outer.c[2]->w - projection) + .length(); + m_result.p[1] = btCross(outer.c[2]->w - projection, + outer.c[0]->w - projection) + .length(); + m_result.p[2] = btCross(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); + } + bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - return false; + if (a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if (a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); } - sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + else if (b_dot_ba < 0) { - 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 = btCross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } - if(v) - { - if(!(getedgedist(face, a, b, face->d) || - getedgedist(face, b, c, face->d) || - getedgedist(face, c, a, face->d))) - { - // Origin projects to the interior of the triangle - // Use distance to triangle plane - face->d = btDot(a->w, face->n) / l; - } + return true; + } - face->n /= l; - if(forced || (face->d >= -EPA_PLANE_EPS)) - { - return face; - } - else - m_status=eStatus::NonConvex; - } - else - m_status=eStatus::Degenerated; + return false; + } + 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 = btCross(b->w - a->w, c->w - a->w); + const btScalar l = face->n.length(); + const bool v = l > EPA_ACCURACY; - remove(m_hull, face); - append(m_stock, face); - return 0; + if (v) + { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = btDot(a->w, face->n) / l; + } + face->n /= l; + if (forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; } - m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; - return 0; + 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; + for (sFace* f = minf->l[1]; f; f = f->l[1]) + { + const btScalar sqd = f->d * f->d; + if (sqd < mind) + { + minf = f; + mind = sqd; } - sFace* findbest() + } + 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 ((btDot(f->n, w->w) - f->d) < -EPA_PLANE_EPS) { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) + sFace* nf = newface(f->c[e1], f->c[e], w, false); + if (nf) { - const btScalar sqd=f->d*f->d; - if(sqd<mind) - { - minf=f; - mind=sqd; - } + 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); } - return(minf); } - bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + else { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) + 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)) { - const U e1=i1m3[e]; - if((btDot(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); - } - } + 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); + } + 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); } +} // namespace gjkepa2_impl + // // Api // -using namespace gjkepa2_impl; +using namespace gjkepa2_impl; // -int btGjkEpaSolver2::StackSizeRequirement() +int btGjkEpaSolver2::StackSizeRequirement() { - return(sizeof(GJK)+sizeof(EPA)); + 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) +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) + 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) + 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; + 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); + 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); + 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) +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) + 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: + case GJK::eStatus::Inside: { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) + 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) + 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]; + 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; + 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; + case GJK::eStatus::Failed: + results.status = sResults::GJK_Failed; + break; default: - { - } + { + } } - return(false); + return (false); } #ifndef __SPU__ // -btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, - btScalar margin, - const btConvexShape* shape0, - const btTransform& wtrs0, - sResults& results) +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) + 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) + 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; + 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->getMarginNonVirtual()+ - shape1.getMarginNonVirtual(); - const btScalar length= delta.length(); - results.normal = delta/length; - results.witnesses[0] += results.normal*margin; - return(length-margin); + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * w1; + const btVector3 delta = results.witnesses[1] - + results.witnesses[0]; + const btScalar margin = shape0->getMarginNonVirtual() + + shape1.getMarginNonVirtual(); + const btScalar length = delta.length(); + results.normal = delta / length; + results.witnesses[0] += results.normal * margin; + results.distance = length - margin; + return results.distance; } else { - if(gjk_status==GJK::eStatus::Inside) + if (gjk_status == GJK::eStatus::Inside) { - if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + if (Penetration(shape0, wtrs0, &shape1, wtrs1, gjk.m_ray, results)) { - const btVector3 delta= results.witnesses[0]- - results.witnesses[1]; - const btScalar length= delta.length(); + const btVector3 delta = results.witnesses[0] - + results.witnesses[1]; + const btScalar length = delta.length(); if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-length); + results.normal = delta / length; + return (-length); } - } + } } - return(SIMD_INFINITY); + return (SIMD_INFINITY); } // -bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) +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)); + if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results)) + return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results, false)); else - return(true); + return (true); } -#endif //__SPU__ +#endif //__SPU__ -/* Symbols cleanup */ +/* Symbols cleanup */ #undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY +#undef GJK_ACCURACY #undef GJK_MIN_DISTANCE #undef GJK_DUPLICATED_EPS #undef GJK_SIMPLEX2_EPS diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h index ac501d5ecfe..893daea3f54 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -28,48 +28,46 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "BulletCollision/CollisionShapes/btConvexShape.h" ///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct btGjkEpaSolver2 +struct btGjkEpaSolver2 { -struct sResults + struct sResults { - enum eStatus + 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; + 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 int StackSizeRequirement(); -static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results); + 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, + static bool Penetration(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, const btVector3& guess, sResults& results, - bool usemargins=true); + bool usemargins = true); #ifndef __SPU__ -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 //__SPU__ + 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 //__SPU__ }; -#endif //BT_GJK_EPA2_H - +#endif //BT_GJK_EPA2_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h index ce1f24bc50d..6fedbbb3e5f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h @@ -29,915 +29,946 @@ Improvements and refactoring by Erwin Coumans, 2008-2014 #include "LinearMath/btTransform.h" #include "btGjkCollisionDescription.h" - - -struct btGjkEpaSolver3 +struct btGjkEpaSolver3 { -struct sResults + struct sResults { - enum eStatus + 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; + 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; }; - - }; - - -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf +#if defined(DEBUG) || defined(_DEBUG) +#include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif +// Config - - // 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 - template <typename btConvexTemplate> - struct MinkowskiDiff - { - const btConvexTemplate* m_convexAPtr; - const btConvexTemplate* m_convexBPtr; - - btMatrix3x3 m_toshape1; - btTransform m_toshape0; - - bool m_enableMargin; - - - MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) - :m_convexAPtr(&a), - m_convexBPtr(&b) - { - } - - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - return m_convexAPtr->getLocalSupportWithMargin(d); - } - inline btVector3 Support1(const btVector3& d) const - { - return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(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)); - } - }; - -enum eGjkStatus +/* 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 +template <typename btConvexTemplate> +struct MinkowskiDiff { - eGjkValid, - eGjkInside, - eGjkFailed + const btConvexTemplate* m_convexAPtr; + const btConvexTemplate* m_convexBPtr; + + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + + bool m_enableMargin; + + MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) + : m_convexAPtr(&a), + m_convexBPtr(&b) + { + } + + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + return m_convexAPtr->getLocalSupportWithMargin(d); + } + inline btVector3 Support1(const btVector3& d) const + { + return m_toshape0 * m_convexBPtr->getLocalSupportWithMargin(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)); + } }; - // GJK - template <typename btConvexTemplate> - struct GJK - { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - - /* Fields */ - - MinkowskiDiff<btConvexTemplate> 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; - eGjkStatus m_status; - /* Methods */ - - GJK(const btConvexTemplate& a, const btConvexTemplate& b) - :m_shape(a,b) - { - Initialize(); - } - void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eGjkFailed; - m_current = 0; - m_distance = 0; - } - eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& 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 = eGjkValid; - 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=eGjkInside; - 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=btDot(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=eGjkInside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed; - } while(m_status==eGjkValid); - m_simplex=&m_simplices[m_current]; - switch(m_status) - { - case eGjkValid: m_distance=m_ray.length();break; - case eGjkInside: m_distance=0;break; - default: - { - } - } - 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=btCross(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=btCross(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?-btDot(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=btCross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) - { - btScalar mindist=-1; - btScalar subw[2]={0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) - { - if(btDot(*vt[i],btCross(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=btDot(a,n); - const btScalar s=btSqrt(l); - const btVector3 p=n*(d/l); - mindist = p.length2(); - m = 7; - w[0] = (btCross(dl[1],b-p)).length()/s; - w[1] = (btCross(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*btDot(a,btCross(b-c,a-b)))<=0; - if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) - { - btScalar mindist=-1; - btScalar subw[3]={0.f,0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*btDot(d,btCross(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); - } - }; +enum eGjkStatus +{ + eGjkValid, + eGjkInside, + eGjkFailed +}; + +// GJK +template <typename btConvexTemplate> +struct GJK +{ + /* Types */ + struct sSV + { + btVector3 d, w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + + /* Fields */ + MinkowskiDiff<btConvexTemplate> 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; + eGjkStatus m_status; + /* Methods */ + + GJK(const btConvexTemplate& a, const btConvexTemplate& b) + : m_shape(a, b) + { + Initialize(); + } + void Initialize() + { + m_ray = btVector3(0, 0, 0); + m_nfree = 0; + m_status = eGjkFailed; + m_current = 0; + m_distance = 0; + } + eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& 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 = eGjkValid; + 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 = eGjkInside; + 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 = btDot(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 = eGjkInside; + } + else + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eGjkFailed; + } while (m_status == eGjkValid); + m_simplex = &m_simplices[m_current]; + switch (m_status) + { + case eGjkValid: + m_distance = m_ray.length(); + break; + case eGjkInside: + m_distance = 0; + break; + default: + { + } + } + 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 = btCross(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 = btCross(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 ? -btDot(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 = btCross(dl[0], dl[1]); + const btScalar l = n.length2(); + if (l > GJK_SIMPLEX3_EPS) + { + btScalar mindist = -1; + btScalar subw[2] = {0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + if (btDot(*vt[i], btCross(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 = btDot(a, n); + const btScalar s = btSqrt(l); + const btVector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1], b - p)).length() / s; + w[1] = (btCross(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 * btDot(a, btCross(b - c, a - b))) <= 0; + if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS)) + { + btScalar mindist = -1; + btScalar subw[3] = {0.f, 0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + const U j = imd3[i]; + const btScalar s = vl * btDot(d, btCross(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); + } +}; -enum eEpaStatus +enum eEpaStatus { - eEpaValid, - eEpaTouching, - eEpaDegenerated, - eEpaNonConvex, - eEpaInvalidHull, - eEpaOutOfFaces, - eEpaOutOfVertices, - eEpaAccuraryReached, - eEpaFallBack, - eEpaFailed + eEpaValid, + eEpaTouching, + eEpaDegenerated, + eEpaNonConvex, + eEpaInvalidHull, + eEpaOutOfFaces, + eEpaOutOfVertices, + eEpaAccuraryReached, + eEpaFallBack, + eEpaFailed }; +// EPA +template <typename btConvexTemplate> +struct EPA +{ + /* Types */ + + struct sFace + { + btVector3 n; + btScalar d; + typename GJK<btConvexTemplate>::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) {} + }; + + /* Fields */ + eEpaStatus m_status; + typename GJK<btConvexTemplate>::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + typename GJK<btConvexTemplate>::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 = eEpaFailed; + 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]); + } + } + eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk, const btVector3& guess) + { + typename GJK<btConvexTemplate>::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 = eEpaValid; + 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 = eEpaValid; + for (; iterations < EPA_MAX_ITERATIONS; ++iterations) + { + if (m_nextsv < EPA_MAX_VERTICES) + { + sHorizon horizon; + typename GJK<btConvexTemplate>::sSV* w = &m_sv_store[m_nextsv++]; + bool valid = true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n, *w); + const btScalar wdist = btDot(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(); + outer = *best; + } + else + { + m_status = eEpaInvalidHull; + break; + } + } + else + { + m_status = eEpaAccuraryReached; + break; + } + } + else + { + m_status = eEpaOutOfVertices; + 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] = btCross(outer.c[1]->w - projection, + outer.c[2]->w - projection) + .length(); + m_result.p[1] = btCross(outer.c[2]->w - projection, + outer.c[0]->w - projection) + .length(); + m_result.p[2] = btCross(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 = eEpaFallBack; + 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); + } + bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if (a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if (a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if (b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } + + return true; + } + + return false; + } + sFace* newface(typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, typename GJK<btConvexTemplate>::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 = btCross(b->w - a->w, c->w - a->w); + const btScalar l = face->n.length(); + const bool v = l > EPA_ACCURACY; + + if (v) + { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = btDot(a->w, face->n) / l; + } + + face->n /= l; + if (forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; + } + else + m_status = eEpaNonConvex; + } + else + m_status = eEpaDegenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + } + m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf = m_hull.root; + btScalar mind = minf->d * minf->d; + for (sFace* f = minf->l[1]; f; f = f->l[1]) + { + const btScalar sqd = f->d * f->d; + if (sqd < mind) + { + minf = f; + mind = sqd; + } + } + return (minf); + } + bool expand(U pass, typename GJK<btConvexTemplate>::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 ((btDot(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); + } +}; - // EPA template <typename btConvexTemplate> - struct EPA - { - /* Types */ - - struct sFace - { - btVector3 n; - btScalar d; - typename GJK<btConvexTemplate>::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) {} - }; - - /* Fields */ - eEpaStatus m_status; - typename GJK<btConvexTemplate>::sSimplex m_result; - btVector3 m_normal; - btScalar m_depth; - typename GJK<btConvexTemplate>::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 = eEpaFailed; - 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]); - } - } - eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess) - { - typename GJK<btConvexTemplate>::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 = eEpaValid; - 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=eEpaValid; - for(;iterations<EPA_MAX_ITERATIONS;++iterations) - { - if(m_nextsv<EPA_MAX_VERTICES) - { - sHorizon horizon; - typename GJK<btConvexTemplate>::sSV* w=&m_sv_store[m_nextsv++]; - bool valid=true; - best->pass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=btDot(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(); - outer=*best; - } else { m_status=eEpaInvalidHull;break; } - } else { m_status=eEpaAccuraryReached;break; } - } else { m_status=eEpaOutOfVertices;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] = btCross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = btCross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = btCross( 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 = eEpaFallBack; - 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); - } - bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist) - { - const btVector3 ba = b->w - a->w; - const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane - const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - - if(a_dot_nab < 0) - { - // Outside of edge a->b - - const btScalar ba_l2 = ba.length2(); - const btScalar a_dot_ba = btDot(a->w, ba); - const btScalar b_dot_ba = btDot(b->w, ba); - - if(a_dot_ba > 0) - { - // Pick distance vertex a - dist = a->w.length(); - } - else if(b_dot_ba < 0) - { - // Pick distance vertex b - dist = b->w.length(); - } - else - { - // Pick distance to edge a->b - const btScalar a_dot_b = btDot(a->w, b->w); - dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); - } - - return true; - } - - return false; - } - sFace* newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::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 = btCross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - - if(v) - { - if(!(getedgedist(face, a, b, face->d) || - getedgedist(face, b, c, face->d) || - getedgedist(face, c, a, face->d))) - { - // Origin projects to the interior of the triangle - // Use distance to triangle plane - face->d = btDot(a->w, face->n) / l; - } - - face->n /= l; - if(forced || (face->d >= -EPA_PLANE_EPS)) - { - return face; - } - else - m_status=eEpaNonConvex; - } - else - m_status=eEpaDegenerated; - - remove(m_hull, face); - append(m_stock, face); - return 0; - - } - m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; - return 0; - } - sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if(sqd<mind) - { - minf=f; - mind=sqd; - } - } - return(minf); - } - bool expand(U pass,typename GJK<btConvexTemplate>::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((btDot(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); - } - - }; - - template <typename btConvexTemplate> - static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b, - btGjkEpaSolver3::sResults& results, - MinkowskiDiff<btConvexTemplate>& shape) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver3::sResults::Separated; - /* Shape */ - - shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); - shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); - - } - +static void Initialize(const btConvexTemplate& a, const btConvexTemplate& b, + btGjkEpaSolver3::sResults& results, + MinkowskiDiff<btConvexTemplate>& shape) +{ + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0, 0, 0); + results.status = btGjkEpaSolver3::sResults::Separated; + /* Shape */ + + shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); + shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); +} // // Api // - - // template <typename btConvexTemplate> -bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, - const btVector3& guess, - btGjkEpaSolver3::sResults& results) +bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) { - MinkowskiDiff<btConvexTemplate> shape(a,b); - Initialize(a,b,results,shape); - GJK<btConvexTemplate> gjk(a,b); - eGjkStatus gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==eGjkValid) - { - 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] = a.getWorldTransform()*w0; - results.witnesses[1] = a.getWorldTransform()*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==eGjkInside? - btGjkEpaSolver3::sResults::Penetrating : - btGjkEpaSolver3::sResults::GJK_Failed ; - return(false); - } + MinkowskiDiff<btConvexTemplate> shape(a, b); + Initialize(a, b, results, shape); + GJK<btConvexTemplate> gjk(a, b); + eGjkStatus gjk_status = gjk.Evaluate(shape, guess); + if (gjk_status == eGjkValid) + { + 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] = a.getWorldTransform() * w0; + results.witnesses[1] = a.getWorldTransform() * 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 == eGjkInside ? btGjkEpaSolver3::sResults::Penetrating : btGjkEpaSolver3::sResults::GJK_Failed; + return (false); + } } - template <typename btConvexTemplate> -bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, - const btConvexTemplate& b, - const btVector3& guess, - btGjkEpaSolver3::sResults& results) +bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, + const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) { - MinkowskiDiff<btConvexTemplate> shape(a,b); - Initialize(a,b,results,shape); - GJK<btConvexTemplate> gjk(a,b); - eGjkStatus gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) - { - case eGjkInside: - { - EPA<btConvexTemplate> epa; - eEpaStatus epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=eEpaFailed) - { - 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 = btGjkEpaSolver3::sResults::Penetrating; - results.witnesses[0] = a.getWorldTransform()*w0; - results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=btGjkEpaSolver3::sResults::EPA_Failed; - } - break; - case eGjkFailed: - results.status=btGjkEpaSolver3::sResults::GJK_Failed; - break; - default: - { - } - } - return(false); + MinkowskiDiff<btConvexTemplate> shape(a, b); + Initialize(a, b, results, shape); + GJK<btConvexTemplate> gjk(a, b); + eGjkStatus gjk_status = gjk.Evaluate(shape, -guess); + switch (gjk_status) + { + case eGjkInside: + { + EPA<btConvexTemplate> epa; + eEpaStatus epa_status = epa.Evaluate(gjk, -guess); + if (epa_status != eEpaFailed) + { + 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 = btGjkEpaSolver3::sResults::Penetrating; + results.witnesses[0] = a.getWorldTransform() * w0; + results.witnesses[1] = a.getWorldTransform() * (w0 - epa.m_normal * epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return (true); + } + else + results.status = btGjkEpaSolver3::sResults::EPA_Failed; + } + break; + case eGjkFailed: + results.status = btGjkEpaSolver3::sResults::GJK_Failed; + break; + default: + { + } + } + return (false); } #if 0 @@ -990,28 +1021,28 @@ int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanc #endif template <typename btConvexTemplate, typename btDistanceInfoTemplate> -int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, - const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) +int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, + const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) { - btGjkEpaSolver3::sResults results; - btVector3 guess = colDesc.m_firstDir; - - bool isSeparated = btGjkEpaSolver3_Distance( a,b, - guess, - results); - if (isSeparated) - { - distInfo->m_distance = results.distance; - distInfo->m_pointOnA= results.witnesses[0]; - distInfo->m_pointOnB= results.witnesses[1]; - distInfo->m_normalBtoA= results.normal; - return 0; - } - - return -1; + btGjkEpaSolver3::sResults results; + btVector3 guess = colDesc.m_firstDir; + + bool isSeparated = btGjkEpaSolver3_Distance(a, b, + guess, + results); + if (isSeparated) + { + distInfo->m_distance = results.distance; + distInfo->m_pointOnA = results.witnesses[0]; + distInfo->m_pointOnB = results.witnesses[1]; + distInfo->m_normalBtoA = results.normal; + return 0; + } + + return -1; } -/* Symbols cleanup */ +/* Symbols cleanup */ #undef GJK_MAX_ITERATIONS #undef GJK_ACCURARY @@ -1029,7 +1060,4 @@ int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, #undef EPA_PLANE_EPS #undef EPA_INSIDE_EPS - - -#endif //BT_GJK_EPA3_H - +#endif //BT_GJK_EPA3_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 572ec36f563..07629229abc 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -18,49 +18,64 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "btGjkEpaPenetrationDepthSolver.h" - #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw) +bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw) { - (void)debugDraw; (void)v; (void)simplexSolver; -// const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); - btGjkEpaSolver2::sResults results; - + btVector3 guessVectors[] = { + btVector3(transformB.getOrigin() - transformA.getOrigin()).safeNormalize(), + btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(), + btVector3(0, 0, 1), + btVector3(0, 1, 0), + btVector3(1, 0, 0), + btVector3(1, 1, 0), + btVector3(1, 1, 1), + btVector3(0, 1, 1), + btVector3(1, 0, 1), + }; - if(btGjkEpaSolver2::Penetration(pConvexA,transformA, - pConvexB,transformB, - guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } else + int numVectors = sizeof(guessVectors) / sizeof(btVector3); + + for (int i = 0; i < numVectors; i++) { - if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + simplexSolver.reset(); + btVector3 guessVector = guessVectors[i]; + + btGjkEpaSolver2::sResults results; + + if (btGjkEpaSolver2::Penetration(pConvexA, transformA, + pConvexB, transformB, + guessVector, results)) + { wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; v = results.normal; - return false; + return true; + } + else + { + if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } } } + //failed to find a distance/penetration + wWitnessOnA.setValue(0, 0, 0); + wWitnessOnB.setValue(0, 0, 0); + v.setValue(0, 0, 0); return false; } - - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 1ed6340af3b..92d6df1729f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -23,21 +23,18 @@ subject to the following restrictions: ///calculate the penetration depth between two convex shapes. class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver { - public : - - btGjkEpaPenetrationDepthSolver() - { - } - - bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw); - - private : - +public: + btGjkEpaPenetrationDepthSolver() + { + } + + bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw); + +private: }; -#endif // BT_GJP_EPA_PENETRATION_DEPTH_H - +#endif // BT_GJP_EPA_PENETRATION_DEPTH_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 759443a9613..5af93cb2fb3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -18,80 +18,685 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" - - -#if defined(DEBUG) || defined (_DEBUG) +#if defined(DEBUG) || defined(_DEBUG) //#define TEST_NON_VIRTUAL 1 -#include <stdio.h> //for debug printf +#include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif //must be above the machine epsilon +#ifdef BT_USE_DOUBLE_PRECISION +#define REL_ERROR2 btScalar(1.0e-12) +btScalar gGjkEpaPenetrationTolerance = 1.0e-12; +#else #define REL_ERROR2 btScalar(1.0e-6) +btScalar gGjkEpaPenetrationTolerance = 0.001; +#endif + -//temp globals, to improve GJK/EPA/penetration calculations -int gNumDeepPenetrationChecks = 0; -int gNumGjkChecks = 0; - - -btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), -m_penetrationDepthSolver(penetrationDepthSolver), -m_simplexSolver(simplexSolver), -m_minkowskiA(objectA), -m_minkowskiB(objectB), -m_shapeTypeA(objectA->getShapeType()), -m_shapeTypeB(objectB->getShapeType()), -m_marginA(objectA->getMargin()), -m_marginB(objectB->getMargin()), -m_ignoreMargin(false), -m_lastUsedMethod(-1), -m_catchDegeneracies(1), -m_fixContactNormalDirection(1) -{ -} -btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), -m_penetrationDepthSolver(penetrationDepthSolver), -m_simplexSolver(simplexSolver), -m_minkowskiA(objectA), -m_minkowskiB(objectB), -m_shapeTypeA(shapeTypeA), -m_shapeTypeB(shapeTypeB), -m_marginA(marginA), -m_marginB(marginB), -m_ignoreMargin(false), -m_lastUsedMethod(-1), -m_catchDegeneracies(1), -m_fixContactNormalDirection(1) -{ -} - -void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver) + : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)), + m_penetrationDepthSolver(penetrationDepthSolver), + m_simplexSolver(simplexSolver), + m_minkowskiA(objectA), + m_minkowskiB(objectB), + m_shapeTypeA(objectA->getShapeType()), + m_shapeTypeB(objectB->getShapeType()), + m_marginA(objectA->getMargin()), + m_marginB(objectB->getMargin()), + m_ignoreMargin(false), + m_lastUsedMethod(-1), + m_catchDegeneracies(1), + m_fixContactNormalDirection(1) +{ +} +btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver) + : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)), + m_penetrationDepthSolver(penetrationDepthSolver), + m_simplexSolver(simplexSolver), + m_minkowskiA(objectA), + m_minkowskiB(objectB), + m_shapeTypeA(shapeTypeA), + m_shapeTypeB(shapeTypeB), + m_marginA(marginA), + m_marginB(marginB), + m_ignoreMargin(false), + m_lastUsedMethod(-1), + m_catchDegeneracies(1), + m_fixContactNormalDirection(1) +{ +} + +void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults) { (void)swapResults; - getClosestPointsNonVirtual(input,output,debugDraw); + getClosestPointsNonVirtual(input, output, debugDraw); +} + +static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb) +{ + btVector3 separatingAxisInA = (dir)*localTransA.getBasis(); + btVector3 separatingAxisInB = (-dir) * localTransB.getBasis(); + + btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + + btVector3 pInA = pInANoMargin; + btVector3 qInB = qInBNoMargin; + + supAworld = localTransA(pInA); + supBworld = localTransB(qInB); + + if (check2d) + { + supAworld[2] = 0.f; + supBworld[2] = 0.f; + } + + aMinb = supAworld - supBworld; +} + +struct btSupportVector +{ + btVector3 v; //!< Support point in minkowski sum + btVector3 v1; //!< Support point in obj1 + btVector3 v2; //!< Support point in obj2 +}; + +struct btSimplex +{ + btSupportVector ps[4]; + int last; //!< index of last added point +}; + +static btVector3 ccd_vec3_origin(0, 0, 0); + +inline void btSimplexInit(btSimplex *s) +{ + s->last = -1; +} + +inline int btSimplexSize(const btSimplex *s) +{ + return s->last + 1; +} + +inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} +inline void btSupportCopy(btSupportVector *d, const btSupportVector *s) +{ + *d = *s; +} + +inline void btVec3Copy(btVector3 *v, const btVector3 *w) +{ + *v = *w; +} + +inline void ccdVec3Add(btVector3 *v, const btVector3 *w) +{ + v->m_floats[0] += w->m_floats[0]; + v->m_floats[1] += w->m_floats[1]; + v->m_floats[2] += w->m_floats[2]; +} + +inline void ccdVec3Sub(btVector3 *v, const btVector3 *w) +{ + *v -= *w; +} +inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) +{ + *d = (*v) - (*w); +} +inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b) +{ + btScalar dot; + dot = a->dot(*b); + + return dot; +} + +inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b) +{ + btVector3 ab; + btVec3Sub2(&ab, a, b); + return btVec3Dot(&ab, &ab); +} + +inline void btVec3Scale(btVector3 *d, btScalar k) +{ + d->m_floats[0] *= k; + d->m_floats[1] *= k; + d->m_floats[2] *= k; +} + +inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) +{ + d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]); + d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]); + d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]); +} + +inline void btTripleCross(const btVector3 *a, const btVector3 *b, + const btVector3 *c, btVector3 *d) +{ + btVector3 e; + btVec3Cross(&e, a, b); + btVec3Cross(d, &e, c); +} + +inline int ccdEq(btScalar _a, btScalar _b) +{ + btScalar ab; + btScalar a, b; + + ab = btFabs(_a - _b); + if (btFabs(ab) < SIMD_EPSILON) + return 1; + + a = btFabs(_a); + b = btFabs(_b); + if (b > a) + { + return ab < SIMD_EPSILON * b; + } + else + { + return ab < SIMD_EPSILON * a; + } +} + +btScalar ccdVec3X(const btVector3 *v) +{ + return v->x(); +} + +btScalar ccdVec3Y(const btVector3 *v) +{ + return v->y(); +} + +btScalar ccdVec3Z(const btVector3 *v) +{ + return v->z(); +} +inline int btVec3Eq(const btVector3 *a, const btVector3 *b) +{ + return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b)); +} + +inline void btSimplexAdd(btSimplex *s, const btSupportVector *v) +{ + // here is no check on boundaries in sake of speed + ++s->last; + btSupportCopy(s->ps + s->last, v); +} + +inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a) +{ + btSupportCopy(s->ps + pos, a); +} + +inline void btSimplexSetSize(btSimplex *s, int size) +{ + s->last = size - 1; +} + +inline const btSupportVector *ccdSimplexLast(const btSimplex *s) +{ + return btSimplexPoint(s, s->last); +} + +inline int ccdSign(btScalar val) +{ + if (btFuzzyZero(val)) + { + return 0; + } + else if (val < btScalar(0)) + { + return -1; + } + return 1; +} + +inline btScalar btVec3PointSegmentDist2(const btVector3 *P, + const btVector3 *x0, + const btVector3 *b, + btVector3 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + btScalar dist, t; + btVector3 d, a; + + // direction of segment + btVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + btVec3Sub2(&a, x0, P); + + t = -btScalar(1.) * btVec3Dot(&a, &d); + t /= btVec3Dot(&d, &d); + + if (t < btScalar(0) || btFuzzyZero(t)) + { + dist = ccdVec3Dist2(x0, P); + if (witness) + btVec3Copy(witness, x0); + } + else if (t > btScalar(1) || ccdEq(t, btScalar(1))) + { + dist = ccdVec3Dist2(b, P); + if (witness) + btVec3Copy(witness, b); + } + else + { + if (witness) + { + btVec3Copy(witness, &d); + btVec3Scale(witness, t); + ccdVec3Add(witness, x0); + dist = ccdVec3Dist2(witness, P); + } + else + { + // recycling variables + btVec3Scale(&d, t); + ccdVec3Add(&d, &a); + dist = btVec3Dot(&d, &d); + } + } + + return dist; +} + +btScalar btVec3PointTriDist2(const btVector3 *P, + const btVector3 *x0, const btVector3 *B, + const btVector3 *C, + btVector3 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + btVector3 d1, d2, a; + double u, v, w, p, q, r; + double s, t, dist, dist2; + btVector3 witness2; + + btVec3Sub2(&d1, B, x0); + btVec3Sub2(&d2, C, x0); + btVec3Sub2(&a, x0, P); + + u = btVec3Dot(&a, &a); + v = btVec3Dot(&d1, &d1); + w = btVec3Dot(&d2, &d2); + p = btVec3Dot(&a, &d1); + q = btVec3Dot(&a, &d2); + r = btVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) + { + if (witness) + { + btVec3Scale(&d1, s); + btVec3Scale(&d2, t); + btVec3Copy(witness, x0); + ccdVec3Add(witness, &d1); + ccdVec3Add(witness, &d2); + + dist = ccdVec3Dist2(witness, P); + } + else + { + dist = s * s * v; + dist += t * t * w; + dist += btScalar(2.) * s * t * r; + dist += btScalar(2.) * s * p; + dist += btScalar(2.) * t * q; + dist += u; + } + } + else + { + dist = btVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + + dist2 = btVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + } + + return dist; +} + +static int btDoSimplex2(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B; + btVector3 AB, AO, tmp; + btScalar dot; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other point + B = btSimplexPoint(simplex, 0); + // compute AB oriented segment + btVec3Sub2(&AB, &B->v, &A->v); + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // dot product AB . AO + dot = btVec3Dot(&AB, &AO); + + // check if origin doesn't lie on AB segment + btVec3Cross(&tmp, &AB, &AO); + if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) + { + return 1; + } + + // check if origin is in area where AB segment is + if (btFuzzyZero(dot) || dot < btScalar(0)) + { + // origin is in outside are of A + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + else + { + // origin is in area where AB segment is + + // keep simplex untouched and set direction to + // AB x AO x AB + btTripleCross(&AB, &AO, &AB, dir); + } + + return 0; +} + +static int btDoSimplex3(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C; + btVector3 AO, AB, AC, ABC, tmp; + btScalar dot, dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 1); + C = btSimplexPoint(simplex, 0); + + // check touching contact + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) + { + return 1; + } + + // check if triangle is really triangle (has area > 0) + // if not simplex can't be expanded and thus no itersection is found + if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) + { + return -1; + } + + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // compute AB and AC segments and ABC vector (perpendircular to triangle) + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + + btVec3Cross(&tmp, &ABC, &AC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + dot = btVec3Dot(&AC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + // C is already in place + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AC, &AO, &AC, dir); + } + else + { + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else + { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + } + else + { + btVec3Cross(&tmp, &AB, &ABC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else + { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + else + { + dot = btVec3Dot(&ABC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btVec3Copy(dir, &ABC); + } + else + { + btSupportVector tmp; + btSupportCopy(&tmp, C); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, &tmp); + + btVec3Copy(dir, &ABC); + btVec3Scale(dir, -btScalar(1)); + } + } + } + + return 0; +} + +static int btDoSimplex4(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C, *D; + btVector3 AO, AB, AC, AD, ABC, ACD, ADB; + int B_on_ACD, C_on_ADB, D_on_ABC; + int AB_O, AC_O, AD_O; + btScalar dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 2); + C = btSimplexPoint(simplex, 1); + D = btSimplexPoint(simplex, 0); + + // check if tetrahedron is really tetrahedron (has volume > 0) + // if it is not simplex can't be expanded and thus no intersection is + // found + dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + { + return -1; + } + + // check if origin lies on some of tetrahedron's face - if so objects + // intersect + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + + // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Sub2(&AD, &D->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + btVec3Cross(&ACD, &AC, &AD); + btVec3Cross(&ADB, &AD, &AB); + + // side (positive or negative) of B, C, D relative to planes ACD, ADB + // and ABC respectively + B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB)); + C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC)); + D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD)); + + // whether origin is on same side of ACD, ADB, ABC as B, C, D + // respectively + AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD; + AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB; + AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC; + + if (AB_O && AC_O && AD_O) + { + // origin is in tetrahedron + return 1; + // rearrange simplex to triangle and call btDoSimplex3() + } + else if (!AB_O) + { + // B is farthest from the origin among all of the tetrahedron's + // points, so remove it from the list and go on with the triangle + // case + + // D and C are in place + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else if (!AC_O) + { + // C is farthest + btSimplexSet(simplex, 1, D); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else + { // (!AD_O) + btSimplexSet(simplex, 0, C); + btSimplexSet(simplex, 1, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + + return btDoSimplex3(simplex, dir); +} + +static int btDoSimplex(btSimplex *simplex, btVector3 *dir) +{ + if (btSimplexSize(simplex) == 2) + { + // simplex contains segment only one segment + return btDoSimplex2(simplex, dir); + } + else if (btSimplexSize(simplex) == 3) + { + // simplex contains triangle + return btDoSimplex3(simplex, dir); + } + else + { // btSimplexSize(simplex) == 4 + // tetrahedron - this is the only shape which can encapsule origin + // so btDoSimplex4() also contains test on it + return btDoSimplex4(simplex, dir); + } } #ifdef __SPU__ -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw) #else -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw) +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw) #endif { m_cachedSeparatingDistance = 0.f; - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar distance = btScalar(0.); + btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.)); - btVector3 pointOnA,pointOnB; - btTransform localTransA = input.m_transformA; + btVector3 pointOnA, pointOnB; + btTransform localTransA = input.m_transformA; btTransform localTransB = input.m_transformB; - btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; @@ -100,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu btScalar marginA = m_marginA; btScalar marginB = m_marginB; - gNumGjkChecks++; //for CCD we don't use margins if (m_ignoreMargin) @@ -110,8 +714,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } m_curIter = 0; - int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? - m_cachedSeparatingAxis.setValue(0,1,0); + int gGjkMaxIter = 1000; //this is to catch invalid input, perhaps check for #NaN? + m_cachedSeparatingAxis.setValue(0, 1, 0); bool isValid = false; bool checkSimplex = false; @@ -119,191 +723,291 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu m_degenerateSimplex = 0; m_lastUsedMethod = -1; - + int status = -2; + btVector3 orgNormalInB(0, 0, 0); + btScalar margin = marginA + marginB; + + //we add a separate implementation to check if the convex shapes intersect + //See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang + //Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver + //and remove this temporary code from libCCD + //this fixes issue https://github.com/bulletphysics/bullet3/issues/1703 + //note, for large differences in shapes, use double precision build! { btScalar squaredDistance = BT_LARGE_FLOAT; btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - m_simplexSolver->reset(); - - for ( ; ; ) - //while (true) - { + btSimplex simplex1; + btSimplex *simplex = &simplex1; + btSimplexInit(simplex); - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); + btVector3 dir(1, 0, 0); + { + btVector3 lastSupV; + btVector3 supAworld; + btVector3 supBworld; + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + btSupportVector last; + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); + btSimplexAdd(simplex, &last); + dir = -lastSupV; - if (check2d) + // start iterations + for (int iterations = 0; iterations < gGjkMaxIter; iterations++) { - pWorld[2] = 0.f; - qWorld[2] = 0.f; - } + // obtain support point + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); + + // check if farthest point in Minkowski difference in direction dir + // isn't somewhere before origin (the test on negative dot product) + // - because if it is, objects are not intersecting at all. + btScalar delta = lastSupV.dot(dir); + if (delta < 0) + { + //no intersection, besides margin + status = -1; + break; + } - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); + // add last support vector to simplex + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) - { - m_degenerateSimplex = 10; - checkSimplex=true; - //checkPenetration = false; - break; - } + btSimplexAdd(simplex, &last); - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (m_simplexSolver->inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * REL_ERROR2; + // if btDoSimplex returns 1 if objects intersect, -1 if objects don't + // intersect and 0 if algorithm should continue - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) + btVector3 newDir; + int do_simplex_res = btDoSimplex(simplex, &dir); + + if (do_simplex_res == 1) + { + status = 0; // intersection found + break; + } + else if (do_simplex_res == -1) + { + // intersection not found + status = -1; + break; + } + + if (btFuzzyZero(btVec3Dot(&dir, &dir))) + { + // intersection not found + status = -1; + } + + if (dir.length2() < SIMD_EPSILON) { - m_degenerateSimplex = 2; - } else + //no intersection, besides margin + status = -1; + break; + } + + if (dir.fuzzyZero()) { - m_degenerateSimplex = 11; + // intersection not found + status = -1; + break; } - checkSimplex = true; - break; } + } - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; + m_simplexSolver->reset(); + if (status == 0) + { + //status = 0; + //printf("Intersect!\n"); + } - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(newCachedSeparatingAxis)) + if (status == -1) + { + //printf("not intersect\n"); + } + //printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]); + if (1) + { + for (;;) + //while (true) { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } + btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis(); + btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis(); - if(newCachedSeparatingAxis.length2()<REL_ERROR2) - { - m_cachedSeparatingAxis = newCachedSeparatingAxis; - m_degenerateSimplex = 6; - checkSimplex = true; - break; - } + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) + { + m_degenerateSimplex = 10; + checkSimplex = true; + //checkPenetration = false; + break; + } + + //exit 0: the new point is already in the simplex, or we didn't come any closer + if (m_simplexSolver->inSimplex(w)) + { + m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * REL_ERROR2; + + if (f0 <= f1) + { + if (f0 <= btScalar(0.)) + { + m_degenerateSimplex = 2; + } + else + { + m_degenerateSimplex = 11; + } + checkSimplex = true; + break; + } + + //add current vertex to simplex + m_simplexSolver->addVertex(w, pWorld, qWorld); + btVector3 newCachedSeparatingAxis; + + //calculate the closest point to the origin (update vector v) + if (!m_simplexSolver->closest(newCachedSeparatingAxis)) + { + m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + if (newCachedSeparatingAxis.length2() < REL_ERROR2) + { + m_cachedSeparatingAxis = newCachedSeparatingAxis; + m_degenerateSimplex = 6; + checkSimplex = true; + break; + } - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = newCachedSeparatingAxis.length2(); + btScalar previousSquaredDistance = squaredDistance; + squaredDistance = newCachedSeparatingAxis.length2(); #if 0 -///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo - if (squaredDistance>previousSquaredDistance) - { - m_degenerateSimplex = 7; - squaredDistance = previousSquaredDistance; - checkSimplex = false; - break; - } -#endif // - - - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); - - //are we getting any closer ? - if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) - { -// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - checkSimplex = true; - m_degenerateSimplex = 12; - - break; - } + ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo + if (squaredDistance > previousSquaredDistance) + { + m_degenerateSimplex = 7; + squaredDistance = previousSquaredDistance; + checkSimplex = false; + break; + } +#endif // - m_cachedSeparatingAxis = newCachedSeparatingAxis; + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + + //are we getting any closer ? + if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) + { + // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + checkSimplex = true; + m_degenerateSimplex = 12; - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { - #if defined(DEBUG) || defined (_DEBUG) + break; + } - printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance, - m_minkowskiA->getShapeType(), - m_minkowskiB->getShapeType()); + m_cachedSeparatingAxis = newCachedSeparatingAxis; - #endif - break; + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (m_curIter++ > gGjkMaxIter) + { +#if defined(DEBUG) || defined(_DEBUG) - } + printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance, + m_minkowskiA->getShapeType(), + m_minkowskiB->getShapeType()); +#endif + break; + } - bool check = (!m_simplexSolver->fullSimplex()); - //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + bool check = (!m_simplexSolver->fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); - if (!check) - { - //do we need this backup_closest here ? -// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - m_degenerateSimplex = 13; - break; + if (!check) + { + //do we need this backup_closest here ? + // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; + break; + } } - } - - if (checkSimplex) - { - m_simplexSolver->compute_points(pointOnA, pointOnB); - normalInB = m_cachedSeparatingAxis; - btScalar lenSqr =m_cachedSeparatingAxis.length2(); - - //valid normal - if (lenSqr < 0.0001) + if (checkSimplex) { - m_degenerateSimplex = 5; - } - if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - normalInB *= rlen; //normalize - - btScalar s = btSqrt(squaredDistance); - - btAssert(s > btScalar(0.0)); - pointOnA -= m_cachedSeparatingAxis * (marginA / s); - pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((btScalar(1.)/rlen) - margin); - isValid = true; - - m_lastUsedMethod = 1; - } else - { - m_lastUsedMethod = 2; + m_simplexSolver->compute_points(pointOnA, pointOnB); + normalInB = m_cachedSeparatingAxis; + + btScalar lenSqr = m_cachedSeparatingAxis.length2(); + + //valid normal + if (lenSqr < REL_ERROR2) + { + m_degenerateSimplex = 5; + } + if (lenSqr > SIMD_EPSILON * SIMD_EPSILON) + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); + normalInB *= rlen; //normalize + + btScalar s = btSqrt(squaredDistance); + + btAssert(s > btScalar(0.0)); + pointOnA -= m_cachedSeparatingAxis * (marginA / s); + pointOnB += m_cachedSeparatingAxis * (marginB / s); + distance = ((btScalar(1.) / rlen) - margin); + isValid = true; + orgNormalInB = normalInB; + + m_lastUsedMethod = 1; + } + else + { + m_lastUsedMethod = 2; + } } } - bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); + bool catchDegeneratePenetrationCase = + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance)); //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0)) { //penetration case @@ -311,149 +1015,169 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu if (m_penetrationDepthSolver) { // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - gNumDeepPenetrationChecks++; + btVector3 tmpPointOnA, tmpPointOnB; + m_cachedSeparatingAxis.setZero(); - bool isValid2 = m_penetrationDepthSolver->calcPenDepth( - *m_simplexSolver, - m_minkowskiA,m_minkowskiB, - localTransA,localTransB, + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( + *m_simplexSolver, + m_minkowskiA, m_minkowskiB, + localTransA, localTransB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw - ); + debugDraw); - - if (isValid2) + if (m_cachedSeparatingAxis.length2()) { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + if (isValid2) { - tmpNormalInB = m_cachedSeparatingAxis; - lenSqr = m_cachedSeparatingAxis.length2(); - } + btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) + { + tmpNormalInB = m_cachedSeparatingAxis; + lenSqr = m_cachedSeparatingAxis.length2(); + } - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); - m_lastUsedMethod = 3; - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) + if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON)) { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - ///todo: need to track down this EPA penetration solver degeneracy - ///the penetration solver reports penetration but the contact normal - ///connecting the contact points is pointing in the opposite direction - ///until then, detect the issue and revert the normal + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length(); + m_lastUsedMethod = 3; + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) { - btScalar d1=0; - { - btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis(); - - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d1 = (-normalInB).dot(w); - } - btScalar d0 = 0.f; - { - btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis(); - - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d0 = normalInB.dot(w); - } - if (d1>d0) - { - m_lastUsedMethod = 10; - normalInB*=-1; - } - + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + } + else + { + m_lastUsedMethod = 8; } - isValid = true; - - } else + } + else { - m_lastUsedMethod = 8; + m_lastUsedMethod = 9; } - } else - { - m_lastUsedMethod = 9; } - } else + else - { - ///this is another degenerate case, where the initial GJK calculation reports a degenerate case - ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) - ///reports a valid positive distance. Use the results of the second GJK instead of failing. - ///thanks to Jacob.Langford for the reproduction case - ///http://code.google.com/p/bullet/issues/detail?id=250 - - - if (m_cachedSeparatingAxis.length2() > btScalar(0.)) { - btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; - //only replace valid distances when the distance is less - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - pointOnA -= m_cachedSeparatingAxis * marginA ; - pointOnB += m_cachedSeparatingAxis * marginB ; - normalInB = m_cachedSeparatingAxis; - normalInB.normalize(); - - isValid = true; - m_lastUsedMethod = 6; - } else + ///this is another degenerate case, where the initial GJK calculation reports a degenerate case + ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) + ///reports a valid positive distance. Use the results of the second GJK instead of failing. + ///thanks to Jacob.Langford for the reproduction case + ///http://code.google.com/p/bullet/issues/detail?id=250 + + if (m_cachedSeparatingAxis.length2() > btScalar(0.)) { - m_lastUsedMethod = 5; + btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin; + //only replace valid distances when the distance is less + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + pointOnA -= m_cachedSeparatingAxis * marginA; + pointOnB += m_cachedSeparatingAxis * marginB; + normalInB = m_cachedSeparatingAxis; + normalInB.normalize(); + + isValid = true; + m_lastUsedMethod = 6; + } + else + { + m_lastUsedMethod = 5; + } } } } - + else + { + //printf("EPA didn't return a valid value\n"); + } } - } } - - - if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) + if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared))) { - m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; + if (1) + { + ///todo: need to track down this EPA penetration solver degeneracy + ///the penetration solver reports penetration but the contact normal + ///connecting the contact points is pointing in the opposite direction + ///until then, detect the issue and revert the normal - output.addContactPoint( - normalInB, - pointOnB+positionOffset, - distance); + btScalar d2 = 0.f; + { + btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis(); + btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis(); - } + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d2 = orgNormalInB.dot(w) - margin; + } -} + btScalar d1 = 0; + { + btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis(); + btVector3 separatingAxisInB = -normalInB * localTransB.getBasis(); + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d1 = (-normalInB).dot(w) - margin; + } + btScalar d0 = 0.f; + { + btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis(); + btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis(); + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d0 = normalInB.dot(w) - margin; + } + if (d1 > d0) + { + m_lastUsedMethod = 10; + normalInB *= -1; + } + + if (orgNormalInB.length2()) + { + if (d2 > d0 && d2 > d1 && d2 > distance) + { + normalInB = orgNormalInB; + distance = d2; + } + } + } + + output.addContactPoint( + normalInB, + pointOnB + positionOffset, + distance); + } + else + { + //printf("invalid gjk query\n"); + } +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index feeae686213..faa02287cae 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -13,9 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - - #ifndef BT_GJK_PAIR_DETECTOR_H #define BT_GJK_PAIR_DETECTOR_H @@ -29,39 +26,34 @@ class btConvexPenetrationDepthSolver; /// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface class btGjkPairDetector : public btDiscreteCollisionDetectorInterface { - - - btVector3 m_cachedSeparatingAxis; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + btVector3 m_cachedSeparatingAxis; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; btSimplexSolverInterface* m_simplexSolver; const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; - int m_shapeTypeA; + int m_shapeTypeA; int m_shapeTypeB; - btScalar m_marginA; - btScalar m_marginB; + btScalar m_marginA; + btScalar m_marginB; - bool m_ignoreMargin; - btScalar m_cachedSeparatingDistance; - + bool m_ignoreMargin; + btScalar m_cachedSeparatingDistance; public: - //some debugging to fix degeneracy problems - int m_lastUsedMethod; - int m_curIter; - int m_degenerateSimplex; - int m_catchDegeneracies; - int m_fixContactNormalDirection; + int m_lastUsedMethod; + int m_curIter; + int m_degenerateSimplex; + int m_catchDegeneracies; + int m_fixContactNormalDirection; - btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - virtual ~btGjkPairDetector() {}; + btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); + btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); + virtual ~btGjkPairDetector(){}; - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); - void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); - + void getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw); void setMinkowskiA(const btConvexShape* minkA) { @@ -72,32 +64,30 @@ public: { m_minkowskiB = minkB; } - void setCachedSeperatingAxis(const btVector3& seperatingAxis) + void setCachedSeparatingAxis(const btVector3& separatingAxis) { - m_cachedSeparatingAxis = seperatingAxis; + m_cachedSeparatingAxis = separatingAxis; } const btVector3& getCachedSeparatingAxis() const { return m_cachedSeparatingAxis; } - btScalar getCachedSeparatingDistance() const + btScalar getCachedSeparatingDistance() const { return m_cachedSeparatingDistance; } - void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) + void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) { m_penetrationDepthSolver = penetrationDepthSolver; } ///don't use setIgnoreMargin, it's for Bullet's internal use - void setIgnoreMargin(bool ignoreMargin) + void setIgnoreMargin(bool ignoreMargin) { m_ignoreMargin = ignoreMargin; } - - }; -#endif //BT_GJK_PAIR_DETECTOR_H +#endif //BT_GJK_PAIR_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index e40fb1d3db6..573fc86bf97 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -20,137 +20,155 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #ifdef PFX_USE_FREE_VECTORMATH - #include "physics_effects/base_level/solver/pfx_constraint_row.h" +#include "physics_effects/base_level/solver/pfx_constraint_row.h" typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; #else - // Don't change following order of parameters - ATTRIBUTE_ALIGNED16(struct) btConstraintRow { - btScalar m_normal[3]; - btScalar m_rhs; - btScalar m_jacDiagInv; - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_accumImpulse; - }; - typedef btConstraintRow PfxConstraintRow; -#endif //PFX_USE_FREE_VECTORMATH - - +// Don't change following order of parameters +ATTRIBUTE_ALIGNED16(struct) +btConstraintRow +{ + btScalar m_normal[3]; + btScalar m_rhs; + btScalar m_jacDiagInv; + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_accumImpulse; +}; +typedef btConstraintRow PfxConstraintRow; +#endif //PFX_USE_FREE_VECTORMATH + +enum btContactPointFlags +{ + BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED = 1, + BT_CONTACT_FLAG_HAS_CONTACT_CFM = 2, + BT_CONTACT_FLAG_HAS_CONTACT_ERP = 4, + BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8, + BT_CONTACT_FLAG_FRICTION_ANCHOR = 16, +}; /// ManifoldContactPoint collects and maintains persistent contactpoints. /// used to improve stability and performance of rigidbody dynamics response. class btManifoldPoint +{ +public: + btManifoldPoint() + : m_userPersistentData(0), + m_contactPointFlags(0), + m_appliedImpulse(0.f), + m_prevRHS(0.f), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), + m_lifeTime(0) { - public: - btManifoldPoint() - :m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), - m_lifeTime(0) - { - } - - btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB, - const btVector3 &normal, - btScalar distance ) : - m_localPointA( pointA ), - m_localPointB( pointB ), - m_normalWorldOnB( normal ), - m_distance1( distance ), - m_combinedFriction(btScalar(0.)), - m_combinedRollingFriction(btScalar(0.)), - m_combinedRestitution(btScalar(0.)), - m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), - m_lifeTime(0) - { - - } - - - - btVector3 m_localPointA; - btVector3 m_localPointB; - btVector3 m_positionWorldOnB; - ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity - btVector3 m_positionWorldOnA; - btVector3 m_normalWorldOnB; - - btScalar m_distance1; - btScalar m_combinedFriction; - btScalar m_combinedRollingFriction; - btScalar m_combinedRestitution; - - //BP mod, store contact triangles. - int m_partId0; - int m_partId1; - int m_index0; - int m_index1; - - mutable void* m_userPersistentData; - bool m_lateralFrictionInitialized; - - btScalar m_appliedImpulse; - btScalar m_appliedImpulseLateral1; - btScalar m_appliedImpulseLateral2; - btScalar m_contactMotion1; - btScalar m_contactMotion2; - btScalar m_contactCFM1; - btScalar m_contactCFM2; - - int m_lifeTime;//lifetime of the contactpoint in frames - - btVector3 m_lateralFrictionDir1; - btVector3 m_lateralFrictionDir2; - - - - - btScalar getDistance() const - { - return m_distance1; - } - int getLifeTime() const - { - return m_lifeTime; - } - - const btVector3& getPositionWorldOnA() const { - return m_positionWorldOnA; -// return m_positionWorldOnB + m_normalWorldOnB * m_distance1; - } - - const btVector3& getPositionWorldOnB() const - { - return m_positionWorldOnB; - } - - void setDistance(btScalar dist) - { - m_distance1 = dist; - } - - ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver - btScalar getAppliedImpulse() const - { - return m_appliedImpulse; - } - - + } + + btManifoldPoint(const btVector3& pointA, const btVector3& pointB, + const btVector3& normal, + btScalar distance) : m_localPointA(pointA), + m_localPointB(pointB), + m_normalWorldOnB(normal), + m_distance1(distance), + m_combinedFriction(btScalar(0.)), + m_combinedRollingFriction(btScalar(0.)), + m_combinedSpinningFriction(btScalar(0.)), + m_combinedRestitution(btScalar(0.)), + m_userPersistentData(0), + m_contactPointFlags(0), + m_appliedImpulse(0.f), + m_prevRHS(0.f), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), + m_lifeTime(0) + { + } + + btVector3 m_localPointA; + btVector3 m_localPointB; + btVector3 m_positionWorldOnB; + ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity + btVector3 m_positionWorldOnA; + btVector3 m_normalWorldOnB; + + btScalar m_distance1; + btScalar m_combinedFriction; + btScalar m_combinedRollingFriction; //torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever + btScalar m_combinedSpinningFriction; //torsional friction around contact normal, useful for grasping objects + btScalar m_combinedRestitution; + + //BP mod, store contact triangles. + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; + + mutable void* m_userPersistentData; + //bool m_lateralFrictionInitialized; + int m_contactPointFlags; + + btScalar m_appliedImpulse; + btScalar m_prevRHS; + btScalar m_appliedImpulseLateral1; + btScalar m_appliedImpulseLateral2; + btScalar m_contactMotion1; + btScalar m_contactMotion2; + + union { + btScalar m_contactCFM; + btScalar m_combinedContactStiffness1; + }; + union { + btScalar m_contactERP; + btScalar m_combinedContactDamping1; }; -#endif //BT_MANIFOLD_CONTACT_POINT_H + btScalar m_frictionCFM; + + int m_lifeTime; //lifetime of the contactpoint in frames + + btVector3 m_lateralFrictionDir1; + btVector3 m_lateralFrictionDir2; + + btScalar getDistance() const + { + return m_distance1; + } + int getLifeTime() const + { + return m_lifeTime; + } + + const btVector3& getPositionWorldOnA() const + { + return m_positionWorldOnA; + // return m_positionWorldOnB + m_normalWorldOnB * m_distance1; + } + + const btVector3& getPositionWorldOnB() const + { + return m_positionWorldOnB; + } + + void setDistance(btScalar dist) + { + m_distance1 = dist; + } + + ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver + btScalar getAppliedImpulse() const + { + return m_appliedImpulse; + } +}; + +#endif //BT_MANIFOLD_CONTACT_POINT_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index fa45f49037e..c042c242080 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -21,42 +21,38 @@ subject to the following restrictions: #define NUM_UNITSPHERE_POINTS 42 - bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw - ) + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw) { - (void)v; - - bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); + + bool check2d = convexA->isConvex2d() && convexB->isConvex2d(); struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { - - btIntermediateResult():m_hasResult(false) + btIntermediateResult() : m_hasResult(false) { } - + btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; btScalar m_depth; - bool m_hasResult; + bool m_hasResult; - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { (void)partId0; (void)index0; } - virtual void setShapeIdentifiersB(int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { (void)partId1; (void)index1; } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { m_normalOnBInWorld = normalOnBInWorld; m_pointInWorld = pointInWorld; @@ -68,42 +64,42 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //just take fixed number of orientation, and sample the penetration depth in that direction btScalar minProj = btScalar(BT_LARGE_FLOAT); btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); - btVector3 minA,minB; - btVector3 seperatingAxisInA,seperatingAxisInB; - btVector3 pInA,qInB,pWorld,qWorld,w; + btVector3 minA, minB; + btVector3 separatingAxisInA, separatingAxisInB; + btVector3 pInA, qInB, pWorld, qWorld, w; #ifndef __SPU__ #define USE_BATCHED_SUPPORT 1 #endif #ifdef USE_BATCHED_SUPPORT - btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 separatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 separatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; int numSampleDirections = NUM_UNITSPHERE_POINTS; - for (i=0;i<numSampleDirections;i++) + for (i = 0; i < numSampleDirections; i++) { btVector3 norm = getPenetrationDirections()[i]; - seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; - seperatingAxisInBBatch[i] = norm * transB.getBasis() ; + separatingAxisInABatch[i] = (-norm) * transA.getBasis(); + separatingAxisInBBatch[i] = norm * transB.getBasis(); } { int numPDA = convexA->getNumPreferredPenetrationDirections(); if (numPDA) { - for (int i=0;i<numPDA;i++) + for (int i = 0; i < numPDA; i++) { btVector3 norm; - convexA->getPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; + convexA->getPreferredPenetrationDirection(i, norm); + norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } @@ -113,42 +109,38 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDB = convexB->getNumPreferredPenetrationDirections(); if (numPDB) { - for (int i=0;i<numPDB;i++) + for (int i = 0; i < numPDB; i++) { btVector3 norm; - convexB->getPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; + convexB->getPreferredPenetrationDirection(i, norm); + norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } } + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInABatch, supportVerticesABatch, numSampleDirections); + convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInBBatch, supportVerticesBBatch, numSampleDirections); - - - convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); - convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); - - for (i=0;i<numSampleDirections;i++) + for (i = 0; i < numSampleDirections; i++) { btVector3 norm = getPenetrationDirections()[i]; if (check2d) { norm[2] = 0.f; } - if (norm.length2()>0.01) + if (norm.length2() > 0.01) { - - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; + separatingAxisInA = separatingAxisInABatch[i]; + separatingAxisInB = separatingAxisInBBatch[i]; pInA = supportVerticesABatch[i]; qInB = supportVerticesBBatch[i]; - pWorld = transA(pInA); + pWorld = transA(pInA); qWorld = transB(qInB); if (check2d) { @@ -156,7 +148,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s qWorld[2] = 0.f; } - w = qWorld - pWorld; + w = qWorld - pWorld; btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) @@ -167,7 +159,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minB = qWorld; } } - } + } #else int numSampleDirections = NUM_UNITSPHERE_POINTS; @@ -177,11 +169,11 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDA = convexA->getNumPreferredPenetrationDirections(); if (numPDA) { - for (int i=0;i<numPDA;i++) + for (int i = 0; i < numPDA; i++) { btVector3 norm; - convexA->getPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; + convexA->getPreferredPenetrationDirection(i, norm); + norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } @@ -192,28 +184,28 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDB = convexB->getNumPreferredPenetrationDirections(); if (numPDB) { - for (int i=0;i<numPDB;i++) + for (int i = 0; i < numPDB; i++) { btVector3 norm; - convexB->getPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; + convexB->getPreferredPenetrationDirection(i, norm); + norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } } -#endif // __SPU__ +#endif // __SPU__ - for (int i=0;i<numSampleDirections;i++) + for (int i = 0; i < numSampleDirections; i++) { const btVector3& norm = getPenetrationDirections()[i]; - seperatingAxisInA = (-norm)* transA.getBasis(); - seperatingAxisInB = norm* transB.getBasis(); - pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - pWorld = transA(pInA); + separatingAxisInA = (-norm) * transA.getBasis(); + separatingAxisInB = norm * transB.getBasis(); + pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + pWorld = transA(pInA); qWorld = transB(qInB); - w = qWorld - pWorld; + w = qWorld - pWorld; btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) @@ -224,48 +216,39 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minB = qWorld; } } -#endif //USE_BATCHED_SUPPORT +#endif //USE_BATCHED_SUPPORT //add the margins - minA += minNorm*convexA->getMarginNonVirtual(); - minB -= minNorm*convexB->getMarginNonVirtual(); + minA += minNorm * convexA->getMarginNonVirtual(); + minB -= minNorm * convexB->getMarginNonVirtual(); //no penetration if (minProj < btScalar(0.)) return false; - btScalar extraSeparation = 0.5f;///scale dependent - minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); - - - - + btScalar extraSeparation = 0.5f; ///scale dependent + minProj += extraSeparation + (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); //#define DEBUG_DRAW 1 #ifdef DEBUG_DRAW if (debugDraw) { - btVector3 color(0,1,0); - debugDraw->drawLine(minA,minB,color); - color = btVector3 (1,1,1); - btVector3 vec = minB-minA; + btVector3 color(0, 1, 0); + debugDraw->drawLine(minA, minB, color); + color = btVector3(1, 1, 1); + btVector3 vec = minB - minA; btScalar prj2 = minNorm.dot(vec); - debugDraw->drawLine(minA,minA+(minNorm*minProj),color); - + debugDraw->drawLine(minA, minA + (minNorm * minProj), color); } -#endif //DEBUG_DRAW +#endif //DEBUG_DRAW - - - btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); + btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0); btScalar offsetDist = minProj; btVector3 offset = minNorm * offsetDist; - - btGjkPairDetector::ClosestPointInput input; - + btVector3 newOrg = transA.getOrigin() + offset; btTransform displacedTrans = transA; @@ -273,89 +256,81 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; - + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); //minProj; + btIntermediateResult res; - gjkdet.setCachedSeperatingAxis(-minNorm); - gjkdet.getClosestPoints(input,res,debugDraw); + gjkdet.setCachedSeparatingAxis(-minNorm); + gjkdet.getClosestPoints(input, res, debugDraw); btScalar correctedMinNorm = minProj - res.m_depth; - //the penetration depth is over-estimated, relax it - btScalar penetration_relaxation= btScalar(1.); - minNorm*=penetration_relaxation; - + btScalar penetration_relaxation = btScalar(1.); + minNorm *= penetration_relaxation; if (res.m_hasResult) { - pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; v = minNorm; - + #ifdef DEBUG_DRAW if (debugDraw) { - btVector3 color(1,0,0); - debugDraw->drawLine(pa,pb,color); + btVector3 color(1, 0, 0); + debugDraw->drawLine(pa, pb, color); } -#endif//DEBUG_DRAW - - +#endif //DEBUG_DRAW } return res.m_hasResult; } -btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() +btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() { - static btVector3 sPenetrationDirections[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)) - }; + static btVector3 sPenetrationDirections[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))}; return sPenetrationDirections; } - - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index fd533b4fc33..8e3e3932591 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -23,18 +23,14 @@ subject to the following restrictions: class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver { protected: - - static btVector3* getPenetrationDirections(); + static btVector3* getPenetrationDirections(); public: - - virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw - ); + virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw); }; -#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H - +#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h index a22a0bae66b..534a66d3fa3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h @@ -28,32 +28,31 @@ //#define MPR_AVERAGE_CONTACT_POSITIONS - struct btMprCollisionDescription { - btVector3 m_firstDir; - int m_maxGjkIterations; - btScalar m_maximumDistanceSquared; - btScalar m_gjkRelError2; - - btMprCollisionDescription() - : m_firstDir(0,1,0), - m_maxGjkIterations(1000), - m_maximumDistanceSquared(1e30f), - m_gjkRelError2(1.0e-6) - { - } - virtual ~btMprCollisionDescription() - { - } + btVector3 m_firstDir; + int m_maxGjkIterations; + btScalar m_maximumDistanceSquared; + btScalar m_gjkRelError2; + + btMprCollisionDescription() + : m_firstDir(0, 1, 0), + m_maxGjkIterations(1000), + m_maximumDistanceSquared(1e30f), + m_gjkRelError2(1.0e-6) + { + } + virtual ~btMprCollisionDescription() + { + } }; struct btMprDistanceInfo { - btVector3 m_pointOnA; - btVector3 m_pointOnB; - btVector3 m_normalBtoA; - btScalar m_distance; + btVector3 m_pointOnA; + btVector3 m_pointOnB; + btVector3 m_normalBtoA; + btScalar m_distance; }; #ifdef __cplusplus @@ -67,131 +66,112 @@ struct btMprDistanceInfo #define BT_MPR_TOLERANCE 1E-6f #define BT_MPR_MAX_ITERATIONS 1000 -struct _btMprSupport_t +struct _btMprSupport_t { - btVector3 v; //!< Support point in minkowski sum - btVector3 v1; //!< Support point in obj1 - btVector3 v2; //!< Support point in obj2 + btVector3 v; //!< Support point in minkowski sum + btVector3 v1; //!< Support point in obj1 + btVector3 v2; //!< Support point in obj2 }; typedef struct _btMprSupport_t btMprSupport_t; -struct _btMprSimplex_t +struct _btMprSimplex_t { - btMprSupport_t ps[4]; - int last; //!< index of last added point + btMprSupport_t ps[4]; + int last; //!< index of last added point }; typedef struct _btMprSimplex_t btMprSimplex_t; -inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx) +inline btMprSupport_t *btMprSimplexPointW(btMprSimplex_t *s, int idx) { - return &s->ps[idx]; + return &s->ps[idx]; } inline void btMprSimplexSetSize(btMprSimplex_t *s, int size) { - s->last = size - 1; + s->last = size - 1; } #ifdef DEBUG_MPR -inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index) +inline void btPrintPortalVertex(_btMprSimplex_t *portal, int index) { - printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(), - portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(), - portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z()); + printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(), portal->ps[index].v.y(), portal->ps[index].v.z(), + portal->ps[index].v1.x(), portal->ps[index].v1.y(), portal->ps[index].v1.z(), + portal->ps[index].v2.x(), portal->ps[index].v2.y(), portal->ps[index].v2.z()); } -#endif //DEBUG_MPR - - - +#endif //DEBUG_MPR inline int btMprSimplexSize(const btMprSimplex_t *s) { - return s->last + 1; + return s->last + 1; } - -inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx) +inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx) { - // here is no check on boundaries - return &s->ps[idx]; + // here is no check on boundaries + return &s->ps[idx]; } inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s) { - *d = *s; + *d = *s; } inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a) { - btMprSupportCopy(s->ps + pos, a); + btMprSupportCopy(s->ps + pos, a); } - inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2) { - btMprSupport_t supp; + btMprSupport_t supp; - btMprSupportCopy(&supp, &s->ps[pos1]); - btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); - btMprSupportCopy(&s->ps[pos2], &supp); + btMprSupportCopy(&supp, &s->ps[pos1]); + btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + btMprSupportCopy(&s->ps[pos2], &supp); } - inline int btMprIsZero(float val) { - return BT_MPR_FABS(val) < FLT_EPSILON; + return BT_MPR_FABS(val) < FLT_EPSILON; } - - inline int btMprEq(float _a, float _b) { - float ab; - float a, b; + float ab; + float a, b; - ab = BT_MPR_FABS(_a - _b); - if (BT_MPR_FABS(ab) < FLT_EPSILON) - return 1; + ab = BT_MPR_FABS(_a - _b); + if (BT_MPR_FABS(ab) < FLT_EPSILON) + return 1; - a = BT_MPR_FABS(_a); - b = BT_MPR_FABS(_b); - if (b > a){ - return ab < FLT_EPSILON * b; - }else{ - return ab < FLT_EPSILON * a; - } + a = BT_MPR_FABS(_a); + b = BT_MPR_FABS(_b); + if (b > a) + { + return ab < FLT_EPSILON * b; + } + else + { + return ab < FLT_EPSILON * a; + } } - -inline int btMprVec3Eq(const btVector3* a, const btVector3 *b) +inline int btMprVec3Eq(const btVector3 *a, const btVector3 *b) { - return btMprEq((*a).x(), (*b).x()) - && btMprEq((*a).y(), (*b).y()) - && btMprEq((*a).z(), (*b).z()); + return btMprEq((*a).x(), (*b).x()) && btMprEq((*a).y(), (*b).y()) && btMprEq((*a).z(), (*b).z()); } - - - - - - - - - - template <typename btConvexTemplate> -inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center) +inline void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center) { - center->v1 = a.getObjectCenterInWorld(); - center->v2 = b.getObjectCenterInWorld(); - center->v = center->v1 - center->v2; + center->v2 = b.getObjectCenterInWorld(); + center->v = center->v1 - center->v2; } inline void btMprVec3Set(btVector3 *v, float x, float y, float z) { - v->setValue(x,y,z); + v->setValue(x, y, z); } inline void btMprVec3Add(btVector3 *v, const btVector3 *w) @@ -201,41 +181,38 @@ inline void btMprVec3Add(btVector3 *v, const btVector3 *w) inline void btMprVec3Copy(btVector3 *v, const btVector3 *w) { - *v = *w; + *v = *w; } inline void btMprVec3Scale(btVector3 *d, float k) { - *d *= k; + *d *= k; } inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b) { - float dot; + float dot; - dot = btDot(*a,*b); - return dot; + dot = btDot(*a, *b); + return dot; } - inline float btMprVec3Len2(const btVector3 *v) { - return btMprVec3Dot(v, v); + return btMprVec3Dot(v, v); } inline void btMprVec3Normalize(btVector3 *d) { - float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); - btMprVec3Scale(d, k); + float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); + btMprVec3Scale(d, k); } inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) { - *d = btCross(*a,*b); - + *d = btCross(*a, *b); } - inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) { *d = *v - *w; @@ -243,666 +220,665 @@ inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir) { - btVector3 v2v1, v3v1; + btVector3 v2v1, v3v1; - btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 1)->v); - btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 1)->v); - btMprVec3Cross(dir, &v2v1, &v3v1); - btMprVec3Normalize(dir); + btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 1)->v); + btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 1)->v); + btMprVec3Cross(dir, &v2v1, &v3v1); + btMprVec3Normalize(dir); } - inline int portalEncapsulesOrigin(const btMprSimplex_t *portal, - const btVector3 *dir) + const btVector3 *dir) { - float dot; - dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); - return btMprIsZero(dot) || dot > 0.f; + float dot; + dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); + return btMprIsZero(dot) || dot > 0.f; } inline int portalReachTolerance(const btMprSimplex_t *portal, - const btMprSupport_t *v4, - const btVector3 *dir) + const btMprSupport_t *v4, + const btVector3 *dir) { - float dv1, dv2, dv3, dv4; - float dot1, dot2, dot3; + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; - // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} - dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir); - dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir); - dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir); - dv4 = btMprVec3Dot(&v4->v, dir); + dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir); + dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir); + dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir); + dv4 = btMprVec3Dot(&v4->v, dir); - dot1 = dv4 - dv1; - dot2 = dv4 - dv2; - dot3 = dv4 - dv3; + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; - dot1 = BT_MPR_FMIN(dot1, dot2); - dot1 = BT_MPR_FMIN(dot1, dot3); + dot1 = BT_MPR_FMIN(dot1, dot2); + dot1 = BT_MPR_FMIN(dot1, dot3); - return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; + return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; } inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, - const btMprSupport_t *v4, - const btVector3 *dir) + const btMprSupport_t *v4, + const btVector3 *dir) { - float dot; - dot = btMprVec3Dot(&v4->v, dir); - return btMprIsZero(dot) || dot > 0.f; + float dot; + dot = btMprVec3Dot(&v4->v, dir); + return btMprIsZero(dot) || dot > 0.f; } inline void btExpandPortal(btMprSimplex_t *portal, - const btMprSupport_t *v4) -{ - float dot; - btVector3 v4v0; - - btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v); - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0); - if (dot > 0.f){ - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0); - if (dot > 0.f){ - btMprSimplexSet(portal, 1, v4); - }else{ - btMprSimplexSet(portal, 3, v4); - } - }else{ - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0); - if (dot > 0.f){ - btMprSimplexSet(portal, 2, v4); - }else{ - btMprSimplexSet(portal, 1, v4); - } - } + const btMprSupport_t *v4) +{ + float dot; + btVector3 v4v0; + + btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v); + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f) + { + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f) + { + btMprSimplexSet(portal, 1, v4); + } + else + { + btMprSimplexSet(portal, 3, v4); + } + } + else + { + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f) + { + btMprSimplexSet(portal, 2, v4); + } + else + { + btMprSimplexSet(portal, 1, v4); + } + } } template <typename btConvexTemplate> -inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - const btVector3& dir, btMprSupport_t *supp) +inline void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + const btVector3 &dir, btMprSupport_t *supp) { - btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis(); - btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis(); + btVector3 separatingAxisInA = dir * a.getWorldTransform().getBasis(); + btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis(); - btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA); - btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB); + btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA); + btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB); supp->v1 = a.getWorldTransform()(pInA); supp->v2 = b.getWorldTransform()(qInB); supp->v = supp->v1 - supp->v2; } - template <typename btConvexTemplate> -static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) -{ - btVector3 dir, va, vb; - float dot; - int cont; - - - - // vertex 0 is center of portal - btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0)); - - - // vertex 0 is center of portal - btMprSimplexSetSize(portal, 1); - - - - btVector3 zero = btVector3(0,0,0); - btVector3* org = &zero; - - if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){ - // Portal's center lies on origin (0,0,0) => we know that objects - // intersect but we would need to know penetration info. - // So move center little bit... - btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); - btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); - } - - - // vertex 1 = support in direction of origin - btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); - btMprVec3Scale(&dir, -1.f); - btMprVec3Normalize(&dir); - - - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1)); - - btMprSimplexSetSize(portal, 2); - - // test if origin isn't outside of v1 - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); - - - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - - // vertex 2 - btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, - &btMprSimplexPoint(portal, 1)->v); - if (btMprIsZero(btMprVec3Len2(&dir))){ - if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){ - // origin lies on v1 - return 1; - }else{ - // origin lies on v0-v1 segment - return 2; - } - } - - btMprVec3Normalize(&dir); - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2)); - - - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - btMprSimplexSetSize(portal, 3); - - // vertex 3 direction - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - - // it is better to form portal faces to be oriented "outside" origin - dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); - if (dot > 0.f){ - btMprSimplexSwap(portal, 1, 2); - btMprVec3Scale(&dir, -1.f); - } - - while (btMprSimplexSize(portal) < 4){ - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3)); - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - cont = 0; - - // test if origin is outside (v1, v0, v3) - set v2 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 3)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)){ - btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); - cont = 1; - } - - if (!cont){ - // test if origin is outside (v3, v0, v2) - set v1 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 2)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)){ - btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); - cont = 1; - } - } - - if (cont){ - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - }else{ - btMprSimplexSetSize(portal, 4); - } - } - - return 0; +static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal) +{ + btVector3 dir, va, vb; + float dot; + int cont; + + // vertex 0 is center of portal + btFindOrigin(a, b, colDesc, btMprSimplexPointW(portal, 0)); + + // vertex 0 is center of portal + btMprSimplexSetSize(portal, 1); + + btVector3 zero = btVector3(0, 0, 0); + btVector3 *org = &zero; + + if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)) + { + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); + } + + // vertex 1 = support in direction of origin + btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); + btMprVec3Scale(&dir, -1.f); + btMprVec3Normalize(&dir); + + btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 1)); + + btMprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); + + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + // vertex 2 + btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, + &btMprSimplexPoint(portal, 1)->v); + if (btMprIsZero(btMprVec3Len2(&dir))) + { + if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)) + { + // origin lies on v1 + return 1; + } + else + { + // origin lies on v0-v1 segment + return 2; + } + } + + btMprVec3Normalize(&dir); + btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 2)); + + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + btMprSimplexSetSize(portal, 3); + + // vertex 3 direction + btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&dir, &va, &vb); + btMprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); + if (dot > 0.f) + { + btMprSimplexSwap(portal, 1, 2); + btMprVec3Scale(&dir, -1.f); + } + + while (btMprSimplexSize(portal) < 4) + { + btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 3)); + + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 3)->v); + dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !btMprIsZero(dot)) + { + btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont) + { + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 2)->v); + dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !btMprIsZero(dot)) + { + btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont) + { + btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&dir, &va, &vb); + btMprVec3Normalize(&dir); + } + else + { + btMprSimplexSetSize(portal, 4); + } + } + + return 0; } template <typename btConvexTemplate> -static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) +static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal) { - btVector3 dir; - btMprSupport_t v4; + btVector3 dir; + btMprSupport_t v4; - for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++) - //while (1) + for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++) + //while (1) { - // compute direction outside the portal (from v0 throught v1,v2,v3 - // face) - btPortalDir(portal, &dir); + // compute direction outside the portal (from v0 through v1,v2,v3 + // face) + btPortalDir(portal, &dir); - // test if origin is inside the portal - if (portalEncapsulesOrigin(portal, &dir)) - return 0; + // test if origin is inside the portal + if (portalEncapsulesOrigin(portal, &dir)) + return 0; - // get next support point - - btMprSupport(a,b,colDesc, dir, &v4); + // get next support point + btMprSupport(a, b, colDesc, dir, &v4); - // test if v4 can expand portal to contain origin and if portal - // expanding doesn't reach given tolerance - if (!portalCanEncapsuleOrigin(portal, &v4, &dir) - || portalReachTolerance(portal, &v4, &dir)) + // test if v4 can expand portal to contain origin and if portal + // expanding doesn't reach given tolerance + if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir)) { - return -1; - } + return -1; + } - // v1-v2-v3 triangle must be rearranged to face outside Minkowski - // difference (direction from v0). - btExpandPortal(portal, &v4); - } + // v1-v2-v3 triangle must be rearranged to face outside Minkowski + // difference (direction from v0). + btExpandPortal(portal, &v4); + } - return -1; + return -1; } static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos) { + btVector3 zero = btVector3(0, 0, 0); + btVector3 *origin = &zero; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; - - btVector3 dir; - size_t i; - float b[4], sum, inv; - btVector3 vec, p1, p2; + btVector3 dir; + size_t i; + float b[4], sum, inv; + btVector3 vec, p1, p2; - btPortalDir(portal, &dir); + btPortalDir(portal, &dir); - // use barycentric coordinates of tetrahedron to find origin - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 2)->v); - b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); + // use barycentric coordinates of tetrahedron to find origin + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 2)->v); + b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 2)->v); - b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 2)->v); + b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v, - &btMprSimplexPoint(portal, 1)->v); - b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v, + &btMprSimplexPoint(portal, 1)->v); + b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 1)->v); - b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 1)->v); + b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); sum = b[0] + b[1] + b[2] + b[3]; - if (btMprIsZero(sum) || sum < 0.f){ + if (btMprIsZero(sum) || sum < 0.f) + { b[0] = 0.f; - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 3)->v); - b[1] = btMprVec3Dot(&vec, &dir); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 1)->v); - b[2] = btMprVec3Dot(&vec, &dir); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 2)->v); - b[3] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 3)->v); + b[1] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 1)->v); + b[2] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 2)->v); + b[3] = btMprVec3Dot(&vec, &dir); sum = b[1] + b[2] + b[3]; } inv = 1.f / sum; - btMprVec3Copy(&p1, origin); - btMprVec3Copy(&p2, origin); - for (i = 0; i < 4; i++){ - btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1); - btMprVec3Scale(&vec, b[i]); - btMprVec3Add(&p1, &vec); - - btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2); - btMprVec3Scale(&vec, b[i]); - btMprVec3Add(&p2, &vec); - } - btMprVec3Scale(&p1, inv); - btMprVec3Scale(&p2, inv); + btMprVec3Copy(&p1, origin); + btMprVec3Copy(&p2, origin); + for (i = 0; i < 4; i++) + { + btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1); + btMprVec3Scale(&vec, b[i]); + btMprVec3Add(&p1, &vec); + + btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2); + btMprVec3Scale(&vec, b[i]); + btMprVec3Add(&p2, &vec); + } + btMprVec3Scale(&p1, inv); + btMprVec3Scale(&p2, inv); #ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &p1); - btMprVec3Add(pos, &p2); - btMprVec3Scale(pos, 0.5); + btMprVec3Copy(pos, &p1); + btMprVec3Add(pos, &p2); + btMprVec3Scale(pos, 0.5); #else - btMprVec3Copy(pos, &p2); -#endif//MPR_AVERAGE_CONTACT_POSITIONS + btMprVec3Copy(pos, &p2); +#endif //MPR_AVERAGE_CONTACT_POSITIONS } inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b) { - btVector3 ab; - btMprVec3Sub2(&ab, a, b); - return btMprVec3Len2(&ab); + btVector3 ab; + btMprVec3Sub2(&ab, a, b); + return btMprVec3Len2(&ab); } inline float _btMprVec3PointSegmentDist2(const btVector3 *P, - const btVector3 *x0, - const btVector3 *b, - btVector3 *witness) -{ - // The computation comes from solving equation of segment: - // S(t) = x0 + t.d - // where - x0 is initial point of segment - // - d is direction of segment from x0 (|d| > 0) - // - t belongs to <0, 1> interval - // - // Than, distance from a segment to some point P can be expressed: - // D(t) = |x0 + t.d - P|^2 - // which is distance from any point on segment. Minimization - // of this function brings distance from P to segment. - // Minimization of D(t) leads to simple quadratic equation that's - // solving is straightforward. - // - // Bonus of this method is witness point for free. - - float dist, t; - btVector3 d, a; - - // direction of segment - btMprVec3Sub2(&d, b, x0); - - // precompute vector from P to x0 - btMprVec3Sub2(&a, x0, P); - - t = -1.f * btMprVec3Dot(&a, &d); - t /= btMprVec3Len2(&d); - - if (t < 0.f || btMprIsZero(t)){ - dist = btMprVec3Dist2(x0, P); - if (witness) - btMprVec3Copy(witness, x0); - }else if (t > 1.f || btMprEq(t, 1.f)){ - dist = btMprVec3Dist2(b, P); - if (witness) - btMprVec3Copy(witness, b); - }else{ - if (witness){ - btMprVec3Copy(witness, &d); - btMprVec3Scale(witness, t); - btMprVec3Add(witness, x0); - dist = btMprVec3Dist2(witness, P); - }else{ - // recycling variables - btMprVec3Scale(&d, t); - btMprVec3Add(&d, &a); - dist = btMprVec3Len2(&d); - } - } - - return dist; -} - + const btVector3 *x0, + const btVector3 *b, + btVector3 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + btVector3 d, a; + + // direction of segment + btMprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + btMprVec3Sub2(&a, x0, P); + + t = -1.f * btMprVec3Dot(&a, &d); + t /= btMprVec3Len2(&d); + + if (t < 0.f || btMprIsZero(t)) + { + dist = btMprVec3Dist2(x0, P); + if (witness) + btMprVec3Copy(witness, x0); + } + else if (t > 1.f || btMprEq(t, 1.f)) + { + dist = btMprVec3Dist2(b, P); + if (witness) + btMprVec3Copy(witness, b); + } + else + { + if (witness) + { + btMprVec3Copy(witness, &d); + btMprVec3Scale(witness, t); + btMprVec3Add(witness, x0); + dist = btMprVec3Dist2(witness, P); + } + else + { + // recycling variables + btMprVec3Scale(&d, t); + btMprVec3Add(&d, &a); + dist = btMprVec3Len2(&d); + } + } + return dist; +} inline float btMprVec3PointTriDist2(const btVector3 *P, - const btVector3 *x0, const btVector3 *B, - const btVector3 *C, - btVector3 *witness) -{ - // Computation comes from analytic expression for triangle (x0, B, C) - // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and - // Then equation for distance is: - // D(s, t) = | T(s, t) - P |^2 - // This leads to minimization of quadratic function of two variables. - // The solution from is taken only if s is between 0 and 1, t is - // between 0 and 1 and t + s < 1, otherwise distance from segment is - // computed. - - btVector3 d1, d2, a; - float u, v, w, p, q, r; - float s, t, dist, dist2; - btVector3 witness2; - - btMprVec3Sub2(&d1, B, x0); - btMprVec3Sub2(&d2, C, x0); - btMprVec3Sub2(&a, x0, P); - - u = btMprVec3Dot(&a, &a); - v = btMprVec3Dot(&d1, &d1); - w = btMprVec3Dot(&d2, &d2); - p = btMprVec3Dot(&a, &d1); - q = btMprVec3Dot(&a, &d2); - r = btMprVec3Dot(&d1, &d2); + const btVector3 *x0, const btVector3 *B, + const btVector3 *C, + btVector3 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + btVector3 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + btVector3 witness2; + + btMprVec3Sub2(&d1, B, x0); + btMprVec3Sub2(&d2, C, x0); + btMprVec3Sub2(&a, x0, P); + + u = btMprVec3Dot(&a, &a); + v = btMprVec3Dot(&d1, &d1); + w = btMprVec3Dot(&d2, &d2); + p = btMprVec3Dot(&a, &d1); + q = btMprVec3Dot(&a, &d2); + r = btMprVec3Dot(&d1, &d2); btScalar div = (w * v - r * r); if (btMprIsZero(div)) { - s=-1; - } else + s = -1; + } + else { s = (q * r - w * p) / div; t = (-s * r - q) / w; } - if ((btMprIsZero(s) || s > 0.f) - && (btMprEq(s, 1.f) || s < 1.f) - && (btMprIsZero(t) || t > 0.f) - && (btMprEq(t, 1.f) || t < 1.f) - && (btMprEq(t + s, 1.f) || t + s < 1.f)){ - - if (witness){ - btMprVec3Scale(&d1, s); - btMprVec3Scale(&d2, t); - btMprVec3Copy(witness, x0); - btMprVec3Add(witness, &d1); - btMprVec3Add(witness, &d2); - - dist = btMprVec3Dist2(witness, P); - }else{ - dist = s * s * v; - dist += t * t * w; - dist += 2.f * s * t * r; - dist += 2.f * s * p; - dist += 2.f * t * q; - dist += u; - } - }else{ - dist = _btMprVec3PointSegmentDist2(P, x0, B, witness); - - dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2); - if (dist2 < dist){ - dist = dist2; - if (witness) - btMprVec3Copy(witness, &witness2); - } - - dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); - if (dist2 < dist){ - dist = dist2; - if (witness) - btMprVec3Copy(witness, &witness2); - } - } - - return dist; + if ((btMprIsZero(s) || s > 0.f) && (btMprEq(s, 1.f) || s < 1.f) && (btMprIsZero(t) || t > 0.f) && (btMprEq(t, 1.f) || t < 1.f) && (btMprEq(t + s, 1.f) || t + s < 1.f)) + { + if (witness) + { + btMprVec3Scale(&d1, s); + btMprVec3Scale(&d2, t); + btMprVec3Copy(witness, x0); + btMprVec3Add(witness, &d1); + btMprVec3Add(witness, &d2); + + dist = btMprVec3Dist2(witness, P); + } + else + { + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + } + else + { + dist = _btMprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btMprVec3Copy(witness, &witness2); + } + + dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btMprVec3Copy(witness, &witness2); + } + } + + return dist; } template <typename btConvexTemplate> -static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal, - float *depth, btVector3 *pdir, btVector3 *pos) +static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal, + float *depth, btVector3 *pdir, btVector3 *pos) { - btVector3 dir; - btMprSupport_t v4; - unsigned long iterations; + btVector3 dir; + btMprSupport_t v4; + unsigned long iterations; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; + btVector3 zero = btVector3(0, 0, 0); + btVector3 *origin = &zero; - - iterations = 1UL; - for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++) - //while (1) + iterations = 1UL; + for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++) + //while (1) { - // compute portal direction and obtain next support point - btPortalDir(portal, &dir); - - btMprSupport(a,b,colDesc, dir, &v4); + // compute portal direction and obtain next support point + btPortalDir(portal, &dir); + btMprSupport(a, b, colDesc, dir, &v4); - // reached tolerance -> find penetration info - if (portalReachTolerance(portal, &v4, &dir) - || iterations ==BT_MPR_MAX_ITERATIONS) + // reached tolerance -> find penetration info + if (portalReachTolerance(portal, &v4, &dir) || iterations == BT_MPR_MAX_ITERATIONS) { - *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir); - *depth = BT_MPR_SQRT(*depth); - + *depth = btMprVec3PointTriDist2(origin, &btMprSimplexPoint(portal, 1)->v, &btMprSimplexPoint(portal, 2)->v, &btMprSimplexPoint(portal, 3)->v, pdir); + *depth = BT_MPR_SQRT(*depth); + if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z())) { - *pdir = dir; - } + } btMprVec3Normalize(pdir); - - // barycentric coordinates: - btFindPos(portal, pos); + // barycentric coordinates: + btFindPos(portal, pos); - return; - } + return; + } - btExpandPortal(portal, &v4); + btExpandPortal(portal, &v4); - iterations++; - } + iterations++; + } } -static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos) +static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos) { - // Touching contact on portal's v1 - so depth is zero and direction - // is unimportant and pos can be guessed - *depth = 0.f; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; - + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + btVector3 zero = btVector3(0, 0, 0); + btVector3 *origin = &zero; btMprVec3Copy(dir, origin); #ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); - btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); - btMprVec3Scale(pos, 0.5); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5); #else - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); #endif } static void btFindPenetrSegment(btMprSimplex_t *portal, - float *depth, btVector3 *dir, btVector3 *pos) + float *depth, btVector3 *dir, btVector3 *pos) { - - // Origin lies on v0-v1 segment. - // Depth is distance to v1, direction also and position must be - // computed + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed #ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); - btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); - btMprVec3Scale(pos, 0.5f); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5f); #else - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); -#endif//MPR_AVERAGE_CONTACT_POSITIONS - - btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); - *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); - btMprVec3Normalize(dir); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); +#endif //MPR_AVERAGE_CONTACT_POSITIONS - + btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); + *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); + btMprVec3Normalize(dir); } - template <typename btConvexTemplate> -inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - float *depthOut, btVector3* dirOut, btVector3* posOut) +inline int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + float *depthOut, btVector3 *dirOut, btVector3 *posOut) { - - btMprSimplex_t portal; + btMprSimplex_t portal; + // Phase 1: Portal discovery + int result = btDiscoverPortal(a, b, colDesc, &portal); - // Phase 1: Portal discovery - int result = btDiscoverPortal(a,b,colDesc, &portal); - - //sepAxis[pairIndex] = *pdir;//or -dir? switch (result) { - case 0: + case 0: { // Phase 2: Portal refinement - - result = btRefinePortal(a,b,colDesc, &portal); + + result = btRefinePortal(a, b, colDesc, &portal); if (result < 0) return -1; // Phase 3. Penetration info - btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut); - - + btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut); + break; } - case 1: + case 1: { - // Touching contact on portal's v1. + // Touching contact on portal's v1. btFindPenetrTouch(&portal, depthOut, dirOut, posOut); - result=0; + result = 0; break; } - case 2: + case 2: { - - btFindPenetrSegment( &portal, depthOut, dirOut, posOut); - result=0; + btFindPenetrSegment(&portal, depthOut, dirOut, posOut); + result = 0; break; } - default: + default: { //if (res < 0) //{ - // Origin isn't inside portal - no collision. - result = -1; + // Origin isn't inside portal - no collision. + result = -1; //} } }; - + return result; }; - -template<typename btConvexTemplate, typename btMprDistanceTemplate> -inline int btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const - btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo) +template <typename btConvexTemplate, typename btMprDistanceTemplate> +inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo) { - btVector3 dir,pos; + btVector3 dir, pos; float depth; - int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos); - if (res==0) + int res = btMprPenetration(a, b, colDesc, &depth, &dir, &pos); + if (res == 0) { distInfo->m_distance = -depth; distInfo->m_pointOnB = pos; distInfo->m_normalBtoA = -dir; - distInfo->m_pointOnA = pos-distInfo->m_distance*dir; + distInfo->m_pointOnA = pos - distInfo->m_distance * dir; return 0; } return -1; } - - -#endif //BT_MPR_PENETRATION_H +#endif //BT_MPR_PENETRATION_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 4d92e853d3f..dca3e09267d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -13,84 +13,86 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btPersistentManifold.h" #include "LinearMath/btTransform.h" +#include "LinearMath/btSerializer.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btCollisionObjectData btCollisionObjectDoubleData +#else +#define btCollisionObjectData btCollisionObjectFloatData +#endif -btScalar gContactBreakingThreshold = btScalar(0.02); -ContactDestroyedCallback gContactDestroyedCallback = 0; -ContactProcessedCallback gContactProcessedCallback = 0; +btScalar gContactBreakingThreshold = btScalar(0.02); +ContactDestroyedCallback gContactDestroyedCallback = 0; +ContactProcessedCallback gContactProcessedCallback = 0; +ContactStartedCallback gContactStartedCallback = 0; +ContactEndedCallback gContactEndedCallback = 0; ///gContactCalcArea3Points will approximate the convex hull area using 3 points ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower -bool gContactCalcArea3Points = true; - +bool gContactCalcArea3Points = true; btPersistentManifold::btPersistentManifold() -:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), -m_body0(0), -m_body1(0), -m_cachedPoints (0), -m_index1a(0) + : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), + m_body0(0), + m_body1(0), + m_cachedPoints(0), + m_companionIdA(0), + m_companionIdB(0), + m_index1a(0) { } - - - #ifdef DEBUG_PERSISTENCY #include <stdio.h> -void btPersistentManifold::DebugPersistency() +void btPersistentManifold::DebugPersistency() { int i; - printf("DebugPersistency : numPoints %d\n",m_cachedPoints); - for (i=0;i<m_cachedPoints;i++) + printf("DebugPersistency : numPoints %d\n", m_cachedPoints); + for (i = 0; i < m_cachedPoints; i++) { - printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData); + printf("m_pointCache[%d].m_userPersistentData = %x\n", i, m_pointCache[i].m_userPersistentData); } } -#endif //DEBUG_PERSISTENCY +#endif //DEBUG_PERSISTENCY void btPersistentManifold::clearUserCache(btManifoldPoint& pt) { - void* oldPtr = pt.m_userPersistentData; if (oldPtr) { #ifdef DEBUG_PERSISTENCY int i; int occurance = 0; - for (i=0;i<m_cachedPoints;i++) + for (i = 0; i < m_cachedPoints; i++) { if (m_pointCache[i].m_userPersistentData == oldPtr) { occurance++; - if (occurance>1) + if (occurance > 1) printf("error in clearUserCache\n"); } } - btAssert(occurance<=0); -#endif //DEBUG_PERSISTENCY + btAssert(occurance <= 0); +#endif //DEBUG_PERSISTENCY if (pt.m_userPersistentData && gContactDestroyedCallback) { (*gContactDestroyedCallback)(pt.m_userPersistentData); pt.m_userPersistentData = 0; } - + #ifdef DEBUG_PERSISTENCY DebugPersistency(); #endif } - - } -static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3) +static inline btScalar calcArea4Points(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& p3) { // It calculates possible 3 area constructed from random 4 points and returns the biggest one. - btVector3 a[3],b[3]; + btVector3 a[3], b[3]; a[0] = p0 - p1; a[1] = p0 - p2; a[2] = p0 - p3; @@ -103,100 +105,102 @@ static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,c btVector3 tmp1 = a[1].cross(b[1]); btVector3 tmp2 = a[2].cross(b[2]); - return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); + return btMax(btMax(tmp0.length2(), tmp1.length2()), tmp2.length2()); } -int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) +int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) { - //calculate 4 possible cases areas, and take biggest area - //also need to keep 'deepest' - - int maxPenetrationIndex = -1; + //calculate 4 possible cases areas, and take biggest area + //also need to keep 'deepest' + + int maxPenetrationIndex = -1; #define KEEP_DEEPEST_POINT 1 #ifdef KEEP_DEEPEST_POINT - btScalar maxPenetration = pt.getDistance(); - for (int i=0;i<4;i++) + btScalar maxPenetration = pt.getDistance(); + for (int i = 0; i < 4; i++) + { + if (m_pointCache[i].getDistance() < maxPenetration) { - if (m_pointCache[i].getDistance() < maxPenetration) - { - maxPenetrationIndex = i; - maxPenetration = m_pointCache[i].getDistance(); - } + maxPenetrationIndex = i; + maxPenetration = m_pointCache[i].getDistance(); } -#endif //KEEP_DEEPEST_POINT - - btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); + } +#endif //KEEP_DEEPEST_POINT + + btScalar res0(btScalar(0.)), res1(btScalar(0.)), res2(btScalar(0.)), res3(btScalar(0.)); if (gContactCalcArea3Points) { if (maxPenetrationIndex != 0) { - btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; - btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + btVector3 a0 = pt.m_localPointA - m_pointCache[1].m_localPointA; + btVector3 b0 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA; btVector3 cross = a0.cross(b0); res0 = cross.length2(); } if (maxPenetrationIndex != 1) { - btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + btVector3 a1 = pt.m_localPointA - m_pointCache[0].m_localPointA; + btVector3 b1 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA; btVector3 cross = a1.cross(b1); res1 = cross.length2(); } if (maxPenetrationIndex != 2) { - btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; + btVector3 a2 = pt.m_localPointA - m_pointCache[0].m_localPointA; + btVector3 b2 = m_pointCache[3].m_localPointA - m_pointCache[1].m_localPointA; btVector3 cross = a2.cross(b2); res2 = cross.length2(); } if (maxPenetrationIndex != 3) { - btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; + btVector3 a3 = pt.m_localPointA - m_pointCache[0].m_localPointA; + btVector3 b3 = m_pointCache[2].m_localPointA - m_pointCache[1].m_localPointA; btVector3 cross = a3.cross(b3); res3 = cross.length2(); } - } + } else { - if(maxPenetrationIndex != 0) { - res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + if (maxPenetrationIndex != 0) + { + res0 = calcArea4Points(pt.m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA); } - if(maxPenetrationIndex != 1) { - res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + if (maxPenetrationIndex != 1) + { + res1 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA); } - if(maxPenetrationIndex != 2) { - res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); + if (maxPenetrationIndex != 2) + { + res2 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[3].m_localPointA); } - if(maxPenetrationIndex != 3) { - res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); + if (maxPenetrationIndex != 3) + { + res3 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA); } } - btVector4 maxvec(res0,res1,res2,res3); + btVector4 maxvec(res0, res1, res2, res3); int biggestarea = maxvec.closestAxis4(); return biggestarea; - } - int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const { - btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); + btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); int size = getNumContacts(); int nearestPoint = -1; - for( int i = 0; i < size; i++ ) + for (int i = 0; i < size; i++) { - const btManifoldPoint &mp = m_pointCache[i]; + const btManifoldPoint& mp = m_pointCache[i]; - btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + btVector3 diffA = mp.m_localPointA - newPoint.m_localPointA; const btScalar distToManiPoint = diffA.dot(diffA); - if( distToManiPoint < shortestDist ) + if (distToManiPoint < shortestDist) { shortestDist = distToManiPoint; nearestPoint = i; @@ -211,7 +215,7 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool { btAssert(validContactDistance(newPoint)); } - + int insertIndex = getNumContacts(); if (insertIndex == MANIFOLD_CACHE_SIZE) { @@ -222,84 +226,225 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool insertIndex = 0; #endif clearUserCache(m_pointCache[insertIndex]); - - } else + } + else { m_cachedPoints++; - - } - if (insertIndex<0) - insertIndex=0; + if (insertIndex < 0) + insertIndex = 0; - btAssert(m_pointCache[insertIndex].m_userPersistentData==0); + btAssert(m_pointCache[insertIndex].m_userPersistentData == 0); m_pointCache[insertIndex] = newPoint; return insertIndex; } -btScalar btPersistentManifold::getContactBreakingThreshold() const +btScalar btPersistentManifold::getContactBreakingThreshold() const { return m_contactBreakingThreshold; } - - -void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) +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 + 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--) + for (i = getNumContacts() - 1; i >= 0; i--) { - btManifoldPoint &manifoldPoint = m_pointCache[i]; - manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA ); - manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB ); - manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB); + btManifoldPoint& manifoldPoint = m_pointCache[i]; + manifoldPoint.m_positionWorldOnA = trA(manifoldPoint.m_localPointA); + manifoldPoint.m_positionWorldOnB = trB(manifoldPoint.m_localPointB); + manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB); manifoldPoint.m_lifeTime++; } - /// then + /// then btScalar distance2d; - btVector3 projectedDifference,projectedPoint; - for (i=getNumContacts()-1;i>=0;i--) + btVector3 projectedDifference, projectedPoint; + for (i = getNumContacts() - 1; i >= 0; i--) { - - btManifoldPoint &manifoldPoint = m_pointCache[i]; + btManifoldPoint& manifoldPoint = m_pointCache[i]; //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) if (!validContactDistance(manifoldPoint)) { removeContactPoint(i); - } else + } + else { + //todo: friction anchor may require the contact to be around a bit longer //contact also becomes invalid when relative movement orthogonal to normal exceeds margin projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; distance2d = projectedDifference.dot(projectedDifference); - if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() ) + if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) { removeContactPoint(i); - } else + } + else { //contact point processed callback if (gContactProcessedCallback) - (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1); + (*gContactProcessedCallback)(manifoldPoint, (void*)m_body0, (void*)m_body1); } } } #ifdef DEBUG_PERSISTENCY DebugPersistency(); -#endif // +#endif // } +int btPersistentManifold::calculateSerializeBufferSize() const +{ + return sizeof(btPersistentManifoldData); +} +const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const +{ + btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer; + memset(dataOut, 0, sizeof(btPersistentManifoldData)); + + dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0()); + dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1()); + dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold(); + dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold(); + dataOut->m_numCachedPoints = manifold->getNumContacts(); + dataOut->m_companionIdA = manifold->m_companionIdA; + dataOut->m_companionIdB = manifold->m_companionIdB; + dataOut->m_index1a = manifold->m_index1a; + dataOut->m_objectType = manifold->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + const btManifoldPoint& pt = manifold->getContactPoint(i); + dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse; + dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS; + dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; + dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2; + pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]); + pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]); + dataOut->m_pointCacheDistance[i] = pt.m_distance1; + dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1; + dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1; + dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime; + dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM; + dataOut->m_pointCacheContactERP[i] = pt.m_contactERP; + dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM; + dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags; + dataOut->m_pointCacheIndex0[i] = pt.m_index0; + dataOut->m_pointCacheIndex1[i] = pt.m_index1; + dataOut->m_pointCachePartId0[i] = pt.m_partId0; + dataOut->m_pointCachePartId1[i] = pt.m_partId1; + pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]); + dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction; + pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]); + dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction; + dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction; + dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution; + dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1; + dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2; + } + return btPersistentManifoldDataName; +} +void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } +} - +void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 5026397f67f..0e26da0ebe7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_PERSISTENT_MANIFOLD_H #define BT_PERSISTENT_MANIFOLD_H - #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "btManifoldPoint.h" @@ -24,14 +23,24 @@ class btCollisionObject; #include "LinearMath/btAlignedAllocator.h" struct btCollisionResult; +struct btCollisionObjectDoubleData; +struct btCollisionObjectFloatData; ///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; +#ifndef SWIG +class btPersistentManifold; + typedef bool (*ContactDestroyedCallback)(void* userPersistentData); -typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); -extern ContactDestroyedCallback gContactDestroyedCallback; +typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp, void* body0, void* body1); +typedef void (*ContactStartedCallback)(btPersistentManifold* const& manifold); +typedef void (*ContactEndedCallback)(btPersistentManifold* const& manifold); +extern ContactDestroyedCallback gContactDestroyedCallback; extern ContactProcessedCallback gContactProcessedCallback; +extern ContactStartedCallback gContactStartedCallback; +extern ContactEndedCallback gContactEndedCallback; +#endif //SWIG //the enum starts at 1024 to avoid type conflicts with btTypedConstraint enum btContactManifoldTypes @@ -50,70 +59,74 @@ enum btContactManifoldTypes ///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_ALIGNED128( class) btPersistentManifold : public btTypedObject -//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject +//ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject +ATTRIBUTE_ALIGNED16(class) +btPersistentManifold : public btTypedObject { - btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; /// this two body pointers can point to the physics rigidbody class. const btCollisionObject* m_body0; const btCollisionObject* m_body1; - int m_cachedPoints; + int m_cachedPoints; - btScalar m_contactBreakingThreshold; - btScalar m_contactProcessingThreshold; + btScalar m_contactBreakingThreshold; + btScalar m_contactProcessingThreshold; - /// sort cached points so most isolated points come first - int sortCachedPoints(const btManifoldPoint& pt); + int sortCachedPoints(const btManifoldPoint& pt); - int findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt); + int findContactPoint(const btManifoldPoint* unUsed, int numUnused, const btManifoldPoint& pt); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_companionIdA; - int m_companionIdB; + int m_companionIdA; + int m_companionIdB; int m_index1a; btPersistentManifold(); - btPersistentManifold(const btCollisionObject* body0,const btCollisionObject* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) + btPersistentManifold(const btCollisionObject* body0, const btCollisionObject* body1, int, btScalar contactBreakingThreshold, btScalar contactProcessingThreshold) : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), - m_body0(body0),m_body1(body1),m_cachedPoints(0), - m_contactBreakingThreshold(contactBreakingThreshold), - m_contactProcessingThreshold(contactProcessingThreshold) + m_body0(body0), + m_body1(body1), + m_cachedPoints(0), + m_contactBreakingThreshold(contactBreakingThreshold), + m_contactProcessingThreshold(contactProcessingThreshold), + m_companionIdA(0), + m_companionIdB(0), + m_index1a(0) { } - SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0;} - SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1;} + SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0; } + SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1; } - void setBodies(const btCollisionObject* body0,const btCollisionObject* body1) + void setBodies(const btCollisionObject* body0, const btCollisionObject* body1) { m_body0 = body0; m_body1 = body1; } - void clearUserCache(btManifoldPoint& pt); + void clearUserCache(btManifoldPoint & pt); #ifdef DEBUG_PERSISTENCY - void DebugPersistency(); -#endif // - - SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints;} + void DebugPersistency(); +#endif // + + SIMD_FORCE_INLINE int getNumContacts() const + { + return m_cachedPoints; + } /// the setNumContacts API is usually not used, except when you gather/fill all contacts manually void setNumContacts(int cachedPoints) { m_cachedPoints = cachedPoints; } - SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const { btAssert(index < m_cachedPoints); @@ -127,110 +140,235 @@ public: } ///@todo: get this margin from the current physics / collision environment - btScalar getContactBreakingThreshold() const; + btScalar getContactBreakingThreshold() const; - btScalar getContactProcessingThreshold() const + btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; } - + void setContactBreakingThreshold(btScalar contactBreakingThreshold) { m_contactBreakingThreshold = contactBreakingThreshold; } - void setContactProcessingThreshold(btScalar contactProcessingThreshold) + void setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; } - - - int getCacheEntry(const btManifoldPoint& newPoint) const; - int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false); + int addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive = false); - void removeContactPoint (int index) + void removeContactPoint(int index) { clearUserCache(m_pointCache[index]); int lastUsedIndex = getNumContacts() - 1; -// m_pointCache[index] = m_pointCache[lastUsedIndex]; - if(index != lastUsedIndex) + // m_pointCache[index] = m_pointCache[lastUsedIndex]; + if (index != lastUsedIndex) { - m_pointCache[index] = m_pointCache[lastUsedIndex]; + 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_prevRHS = 0.f; + m_pointCache[lastUsedIndex].m_contactPointFlags = 0; 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); + btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0); m_cachedPoints--; + + if (gContactEndedCallback && m_cachedPoints == 0) + { + gContactEndedCallback(this); + } } - void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) + void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex) { btAssert(validContactDistance(newPoint)); #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; -// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; - - - - btAssert(lifeTime>=0); - void* cache = m_pointCache[insertIndex].m_userPersistentData; - - m_pointCache[insertIndex] = newPoint; + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; + btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; + + bool replacePoint = true; + ///we keep existing contact points for friction anchors + ///if the friction force is within the Coulomb friction cone + if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + // printf("appliedImpulse=%f\n", appliedImpulse); + // printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1); + // printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2); + // printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction); + btScalar mu = m_pointCache[insertIndex].m_combinedFriction; + btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7 + btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2; + btScalar b = eps + mu * appliedImpulse; + b = b * b; + replacePoint = (a) > (b); + } - m_pointCache[insertIndex].m_userPersistentData = cache; - m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; - m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; - m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - + if (replacePoint) + { + 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_prevRHS = prevRHS; + m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; + m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; + } m_pointCache[insertIndex].m_lifeTime = lifeTime; #else clearUserCache(m_pointCache[insertIndex]); m_pointCache[insertIndex] = newPoint; - + #endif } - bool validContactDistance(const btManifoldPoint& pt) const { return pt.m_distance1 <= getContactBreakingThreshold(); } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin - void refreshContactPoints( const btTransform& trA,const btTransform& trB); + void refreshContactPoints(const btTransform& trA, const btTransform& trB); - - SIMD_FORCE_INLINE void clearManifold() + SIMD_FORCE_INLINE void clearManifold() { int i; - for (i=0;i<m_cachedPoints;i++) + for (i = 0; i < m_cachedPoints; i++) { clearUserCache(m_pointCache[i]); } + + if (gContactEndedCallback && m_cachedPoints) + { + gContactEndedCallback(this); + } m_cachedPoints = 0; } + int calculateSerializeBufferSize() const; + const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const; + void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr); + void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr); +}; +// clang-format off -} -; +struct btPersistentManifoldDoubleData +{ + btVector3DoubleData m_pointCacheLocalPointA[4]; + btVector3DoubleData m_pointCacheLocalPointB[4]; + btVector3DoubleData m_pointCachePositionWorldOnA[4]; + btVector3DoubleData m_pointCachePositionWorldOnB[4]; + btVector3DoubleData m_pointCacheNormalWorldOnB[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir1[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir2[4]; + double m_pointCacheDistance[4]; + double m_pointCacheAppliedImpulse[4]; + double m_pointCachePrevRHS[4]; + double m_pointCacheCombinedFriction[4]; + double m_pointCacheCombinedRollingFriction[4]; + double m_pointCacheCombinedSpinningFriction[4]; + double m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + double m_pointCacheAppliedImpulseLateral1[4]; + double m_pointCacheAppliedImpulseLateral2[4]; + double m_pointCacheContactMotion1[4]; + double m_pointCacheContactMotion2[4]; + double m_pointCacheContactCFM[4]; + double m_pointCacheCombinedContactStiffness1[4]; + double m_pointCacheContactERP[4]; + double m_pointCacheCombinedContactDamping1[4]; + double m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + int m_objectType; + double m_contactBreakingThreshold; + double m_contactProcessingThreshold; + int m_padding; + btCollisionObjectDoubleData *m_body0; + btCollisionObjectDoubleData *m_body1; +}; +struct btPersistentManifoldFloatData +{ + btVector3FloatData m_pointCacheLocalPointA[4]; + btVector3FloatData m_pointCacheLocalPointB[4]; + btVector3FloatData m_pointCachePositionWorldOnA[4]; + btVector3FloatData m_pointCachePositionWorldOnB[4]; + btVector3FloatData m_pointCacheNormalWorldOnB[4]; + btVector3FloatData m_pointCacheLateralFrictionDir1[4]; + btVector3FloatData m_pointCacheLateralFrictionDir2[4]; + float m_pointCacheDistance[4]; + float m_pointCacheAppliedImpulse[4]; + float m_pointCachePrevRHS[4]; + float m_pointCacheCombinedFriction[4]; + float m_pointCacheCombinedRollingFriction[4]; + float m_pointCacheCombinedSpinningFriction[4]; + float m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + float m_pointCacheAppliedImpulseLateral1[4]; + float m_pointCacheAppliedImpulseLateral2[4]; + float m_pointCacheContactMotion1[4]; + float m_pointCacheContactMotion2[4]; + float m_pointCacheContactCFM[4]; + float m_pointCacheCombinedContactStiffness1[4]; + float m_pointCacheContactERP[4]; + float m_pointCacheCombinedContactDamping1[4]; + float m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + + int m_objectType; + float m_contactBreakingThreshold; + float m_contactProcessingThreshold; + int m_padding; + + btCollisionObjectFloatData *m_body0; + btCollisionObjectFloatData *m_body1; +}; + +// clang-format on + +#ifdef BT_USE_DOUBLE_PRECISION +#define btPersistentManifoldData btPersistentManifoldDoubleData +#define btPersistentManifoldDataName "btPersistentManifoldDoubleData" +#else +#define btPersistentManifoldData btPersistentManifoldFloatData +#define btPersistentManifoldDataName "btPersistentManifoldFloatData" +#endif //BT_USE_DOUBLE_PRECISION -#endif //BT_PERSISTENT_MANIFOLD_H +#endif //BT_PERSISTENT_MANIFOLD_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index 18da171011a..0900eb6e85a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -18,38 +18,33 @@ subject to the following restrictions: #include "btDiscreteCollisionDetectorInterface.h" - - struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result { - - btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; - btScalar m_distance;//negative means penetration + btScalar m_distance; //negative means penetration - bool m_hasResult; + bool m_hasResult; - btPointCollector () - : m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false) + btPointCollector() + : m_distance(btScalar(BT_LARGE_FLOAT)), m_hasResult(false) { } - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { (void)partId0; (void)index0; - } - virtual void setShapeIdentifiersB(int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { (void)partId1; (void)index1; } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { - if (depth< m_distance) + if (depth < m_distance) { m_hasResult = true; m_normalOnBInWorld = normalOnBInWorld; @@ -60,5 +55,4 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result } }; -#endif //BT_POINT_COLLECTOR_H - +#endif //BT_POINT_COLLECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index d5f4a964bf3..9d1836037d5 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -13,45 +13,42 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans ///Separating axis rest based on work from Pierre Terdiman, see ///And contact clipping based on work from Simon Hobbs - #include "btPolyhedralContactClipping.h" #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" -#include <float.h> //for FLT_MAX +#include <float.h> //for FLT_MAX -int gExpectedNbTests=0; +int gExpectedNbTests = 0; int gActualNbTests = 0; bool gUseInternalObject = true; // Clips a face to the back of a plane -void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS) +void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS) { - int ve; btScalar ds, de; int numVerts = pVtxIn.size(); if (numVerts < 2) return; - btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; + btVector3 firstVertex = pVtxIn[pVtxIn.size() - 1]; btVector3 endVertex = pVtxIn[0]; - - ds = planeNormalWS.dot(firstVertex)+planeEqWS; + + ds = planeNormalWS.dot(firstVertex) + planeEqWS; for (ve = 0; ve < numVerts; ve++) { - endVertex=pVtxIn[ve]; + endVertex = pVtxIn[ve]; - de = planeNormalWS.dot(endVertex)+planeEqWS; + de = planeNormalWS.dot(endVertex) + planeEqWS; - if (ds<0) + if (ds < 0) { - if (de<0) + if (de < 0) { // Start < 0, end < 0, so output endVertex ppVtxOut.push_back(endVertex); @@ -59,15 +56,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex else { // Start < 0, end >= 0, so output intersection - ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); } } else { - if (de<0) + if (de < 0) { // Start >= 0, end < 0 so output intersection and end - ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); ppVtxOut.push_back(endVertex); } } @@ -76,47 +73,44 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex } } - -static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) +static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) { - btScalar Min0,Max0; - btScalar Min1,Max1; - btVector3 witnesPtMinA,witnesPtMaxA; - btVector3 witnesPtMinB,witnesPtMaxB; + btScalar Min0, Max0; + btScalar Min1, Max1; + btVector3 witnesPtMinA, witnesPtMaxA; + btVector3 witnesPtMinB, witnesPtMaxB; - hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); - hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); + hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA); + hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB); - if(Max0<Min1 || Max1<Min0) + if (Max0 < Min1 || Max1 < Min0) return false; btScalar d0 = Max0 - Min1; - btAssert(d0>=0.0f); + btAssert(d0 >= 0.0f); btScalar d1 = Max1 - Min0; - btAssert(d1>=0.0f); - if (d0<d1) + btAssert(d1 >= 0.0f); + if (d0 < d1) { depth = d0; witnessPointA = witnesPtMaxA; witnessPointB = witnesPtMinB; - - } else + } + else { depth = d1; witnessPointA = witnesPtMinA; witnessPointB = witnesPtMaxB; } - + return true; } - - -static int gActualSATPairTests=0; +static int gActualSATPairTests = 0; inline bool IsAlmostZero(const btVector3& v) { - if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false; + if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; } @@ -125,9 +119,9 @@ inline bool IsAlmostZero(const btVector3& v) inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3]) { // This version is ~11.000 cycles (4%) faster overall in one of the tests. -// IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); -// IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); -// IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); + // IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); + // IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); + // IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); p[0] = sv[0] < 0.0f ? -extents[0] : extents[0]; p[1] = sv[1] < 0.0f ? -extents[1] : extents[1]; p[2] = sv[2] < 0.0f ? -extents[2] : extents[2]; @@ -140,90 +134,94 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans const btVector3& r1 = rot[1]; const btVector3& r2 = rot[2]; - const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z(); - const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z(); - const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z(); + const btScalar x = r0.x() * in.x() + r1.x() * in.y() + r2.x() * in.z(); + const btScalar y = r0.y() * in.x() + r1.y() * in.y() + r2.y() * in.z(); + const btScalar z = r0.z() * in.x() + r1.z() * in.y() + r2.z() * in.z(); out.setValue(x, y, z); } - bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) +bool TestInternalObjects(const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) { const btScalar dp = delta_c.dot(axis); btVector3 localAxis0; - InverseTransformPoint3x3(localAxis0, axis,trans0); + InverseTransformPoint3x3(localAxis0, axis, trans0); btVector3 localAxis1; - InverseTransformPoint3x3(localAxis1, axis,trans1); + InverseTransformPoint3x3(localAxis1, axis, trans1); btScalar p0[3]; BoxSupport(convex0.m_extents, localAxis0, p0); btScalar p1[3]; BoxSupport(convex1.m_extents, localAxis1, p1); - const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z(); - const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z(); + const btScalar Radius0 = p0[0] * localAxis0.x() + p0[1] * localAxis0.y() + p0[2] * localAxis0.z(); + const btScalar Radius1 = p1[0] * localAxis1.x() + p1[1] * localAxis1.y() + p1[2] * localAxis1.z(); - const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius; - const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius; + const btScalar MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius; + const btScalar MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius; const btScalar MinMaxRadius = MaxRadius + MinRadius; const btScalar d0 = MinMaxRadius + dp; const btScalar d1 = MinMaxRadius - dp; - const btScalar depth = d0<d1 ? d0:d1; - if(depth>dmin) + const btScalar depth = d0 < d1 ? d0 : d1; + if (depth > dmin) return false; return true; } -#endif //TEST_INTERNAL_OBJECTS +#endif //TEST_INTERNAL_OBJECTS - - - SIMD_FORCE_INLINE void btSegmentsClosestPoints( +SIMD_FORCE_INLINE void btSegmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, btVector3& offsetB, btScalar& tA, btScalar& tB, const btVector3& translation, const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) + const btVector3& dirB, btScalar hlenB) { // compute the parameters of the closest points on each line segment - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); + btScalar dirA_dot_dirB = btDot(dirA, dirB); + btScalar dirA_dot_trans = btDot(dirA, translation); + btScalar dirB_dot_trans = btDot(dirB, translation); btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - if ( denom == 0.0f ) { + if (denom == 0.0f) + { tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) + } + else + { + tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom; + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } tB = tA * dirA_dot_dirB - dirB_dot_trans; - if ( tB < -hlenB ) { + if (tB < -hlenB) + { tB = -hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; - } else if ( tB > hlenB ) { + } + else if (tB > hlenB) + { tB = hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } @@ -235,44 +233,42 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans ptsVector = translation - offsetA + offsetB; } - - -bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) +bool btPolyhedralContactClipping::findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) { gActualSATPairTests++; -//#ifdef TEST_INTERNAL_OBJECTS + //#ifdef TEST_INTERNAL_OBJECTS const btVector3 c0 = transA * hullA.m_localCenter; const btVector3 c1 = transB * hullB.m_localCenter; const btVector3 DeltaC2 = c0 - c1; -//#endif + //#endif btScalar dmin = FLT_MAX; - int curPlaneTests=0; + int curPlaneTests = 0; int numFacesA = hullA.m_faces.size(); // Test normals from hullA - for(int i=0;i<numFacesA;i++) + for (int i = 0; i < numFacesA; i++) { const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]); btVector3 faceANormalWS = transA.getBasis() * Normal; - if (DeltaC2.dot(faceANormalWS)<0) - faceANormalWS*=-1.f; + if (DeltaC2.dot(faceANormalWS) < 0) + faceANormalWS *= -1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; - btVector3 wA,wB; - if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, faceANormalWS, d, wA, wB)) return false; - if(d<dmin) + if (d < dmin) { dmin = d; sep = faceANormalWS; @@ -281,92 +277,89 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& int numFacesB = hullB.m_faces.size(); // Test normals from hullB - for(int i=0;i<numFacesB;i++) + for (int i = 0; i < numFacesB; i++) { const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]); btVector3 WorldNormal = transB.getBasis() * Normal; - if (DeltaC2.dot(WorldNormal)<0) - WorldNormal *=-1.f; + if (DeltaC2.dot(WorldNormal) < 0) + WorldNormal *= -1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; - btVector3 wA,wB; - if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d, wA, wB)) return false; - if(d<dmin) + if (d < dmin) { dmin = d; sep = WorldNormal; } } - btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend; - int edgeA=-1; - int edgeB=-1; + btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend; + int edgeA = -1; + int edgeB = -1; btVector3 worldEdgeA; btVector3 worldEdgeB; - btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0); - + btVector3 witnessPointA(0, 0, 0), witnessPointB(0, 0, 0); int curEdgeEdge = 0; // Test edges - for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++) + for (int e0 = 0; e0 < hullA.m_uniqueEdges.size(); e0++) { const btVector3 edge0 = hullA.m_uniqueEdges[e0]; const btVector3 WorldEdge0 = transA.getBasis() * edge0; - for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++) + for (int e1 = 0; e1 < hullB.m_uniqueEdges.size(); e1++) { const btVector3 edge1 = hullB.m_uniqueEdges[e1]; const btVector3 WorldEdge1 = transB.getBasis() * edge1; btVector3 Cross = WorldEdge0.cross(WorldEdge1); curEdgeEdge++; - if(!IsAlmostZero(Cross)) + if (!IsAlmostZero(Cross)) { Cross = Cross.normalize(); - if (DeltaC2.dot(Cross)<0) + if (DeltaC2.dot(Cross) < 0) Cross *= -1.f; - #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar dist; - btVector3 wA,wB; - if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, Cross, dist, wA, wB)) return false; - if(dist<dmin) + if (dist < dmin) { dmin = dist; sep = Cross; - edgeA=e0; - edgeB=e1; + edgeA = e0; + edgeB = e1; worldEdgeA = WorldEdge0; worldEdgeB = WorldEdge1; - witnessPointA=wA; - witnessPointB=wB; + witnessPointA = wA; + witnessPointB = wB; } } } - } - if (edgeA>=0&&edgeB>=0) + if (edgeA >= 0 && edgeB >= 0) { -// printf("edge-edge\n"); + // printf("edge-edge\n"); //add an edge-edge contact btVector3 ptsVector; @@ -375,57 +368,55 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& btScalar tA; btScalar tB; - btVector3 translation = witnessPointB-witnessPointA; + btVector3 translation = witnessPointB - witnessPointA; btVector3 dirA = worldEdgeA; btVector3 dirB = worldEdgeB; - + btScalar hlenB = 1e30f; btScalar hlenA = 1e30f; - btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB, - translation, - dirA, hlenA, - dirB,hlenB); + btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, + translation, + dirA, hlenA, + dirB, hlenB); btScalar nlSqrt = ptsVector.length2(); - if (nlSqrt>SIMD_EPSILON) + if (nlSqrt > SIMD_EPSILON) { btScalar nl = btSqrt(nlSqrt); - ptsVector *= 1.f/nl; - if (ptsVector.dot(DeltaC2)<0.f) + ptsVector *= 1.f / nl; + if (ptsVector.dot(DeltaC2) < 0.f) { - ptsVector*=-1.f; + ptsVector *= -1.f; } btVector3 ptOnB = witnessPointB + offsetB; btScalar distance = nl; - resultOut.addContactPoint(ptsVector, ptOnB,-distance); + resultOut.addContactPoint(ptsVector, ptOnB, -distance); } - } - - if((DeltaC2.dot(sep))<0.0f) + if ((DeltaC2.dot(sep)) < 0.0f) sep = -sep; return true; } -void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut) { - btVertexArray worldVertsB2; + worldVertsB2.resize(0); btVertexArray* pVtxIn = &worldVertsB1; btVertexArray* pVtxOut = &worldVertsB2; pVtxOut->reserve(pVtxIn->size()); - int closestFaceA=-1; + int closestFaceA = -1; { btScalar dmin = FLT_MAX; - for(int face=0;face<hullA.m_faces.size();face++) + for (int face = 0; face < hullA.m_faces.size(); face++) { const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]); const btVector3 faceANormalWS = transA.getBasis() * Normal; - + btScalar d = faceANormalWS.dot(separatingNormal); if (d < dmin) { @@ -434,69 +425,66 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin } } } - if (closestFaceA<0) + if (closestFaceA < 0) return; const btFace& polyA = hullA.m_faces[closestFaceA]; - // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face int numVerticesA = polyA.m_indices.size(); - for(int e0=0;e0<numVerticesA;e0++) + for (int e0 = 0; e0 < numVerticesA; e0++) { const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]]; - const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]]; + const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]]; const btVector3 edge0 = a - b; const btVector3 WorldEdge0 = transA.getBasis() * edge0; - btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btVector3 worldPlaneAnormal1 = transA.getBasis() * btVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); - btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0); - btVector3 worldA1 = transA*a; + btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1); //.cross(WorldEdge0); + btVector3 worldA1 = transA * a; btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1); - + //int otherFace=0; #ifdef BLA1 int otherFace = polyA.m_connectedFaces[e0]; - btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]); + btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]); btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); -#else + btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; + btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); +#else btVector3 planeNormalWS = planeNormalWS1; - btScalar planeEqWS=planeEqWS1; - + btScalar planeEqWS = planeEqWS1; + #endif //clip face - clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); - btSwap(pVtxIn,pVtxOut); + clipFace(*pVtxIn, *pVtxOut, planeNormalWS, planeEqWS); + btSwap(pVtxIn, pVtxOut); pVtxOut->resize(0); } - - -//#define ONLY_REPORT_DEEPEST_POINT + //#define ONLY_REPORT_DEEPEST_POINT btVector3 point; - // only keep points that are behind the witness face { - btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btVector3 localPlaneNormal(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); btScalar localPlaneEq = polyA.m_plane[3]; - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); - for (int i=0;i<pVtxIn->size();i++) + btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; + btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); + for (int i = 0; i < pVtxIn->size(); i++) { btVector3 vtx = pVtxIn->at(i); - btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; - if (depth <=minDist) + btScalar depth = planeNormalWS.dot(vtx) + planeEqWS; + if (depth <= minDist) { -// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); + // printf("clamped: depth=%f to minDist=%f\n",depth,minDist); depth = minDist; } - if (depth <=maxDist) + if (depth <= maxDist) { btVector3 point = pVtxIn->at(i); #ifdef ONLY_REPORT_DEEPEST_POINT @@ -507,40 +495,32 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin { printf("error in btPolyhedralContactClipping depth = %f\n", depth); printf("likely wrong separatingNormal passed in\n"); - } -#endif - resultOut.addContactPoint(separatingNormal,point,depth); + } +#endif + resultOut.addContactPoint(separatingNormal, point, depth); #endif } } } #ifdef ONLY_REPORT_DEEPEST_POINT - if (curMaxDist<maxDist) + if (curMaxDist < maxDist) { - resultOut.addContactPoint(separatingNormal,point,curMaxDist); + resultOut.addContactPoint(separatingNormal, point, curMaxDist); } -#endif //ONLY_REPORT_DEEPEST_POINT - +#endif //ONLY_REPORT_DEEPEST_POINT } - - - - -void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut) { - btVector3 separatingNormal = separatingNormal1.normalized(); -// const btVector3 c0 = transA * hullA.m_localCenter; -// const btVector3 c1 = transB * hullB.m_localCenter; + // const btVector3 c0 = transA * hullA.m_localCenter; + // const btVector3 c1 = transB * hullB.m_localCenter; //const btVector3 DeltaC2 = c0 - c1; - - - int closestFaceB=-1; + int closestFaceB = -1; btScalar dmax = -FLT_MAX; { - for(int face=0;face<hullB.m_faces.size();face++) + for (int face = 0; face < hullB.m_faces.size(); face++) { const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]); const btVector3 WorldNormal = transB.getBasis() * Normal; @@ -552,19 +532,17 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin } } } - btVertexArray worldVertsB1; - { - const btFace& polyB = hullB.m_faces[closestFaceB]; - const int numVertices = polyB.m_indices.size(); - for(int e0=0;e0<numVertices;e0++) - { - const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; - worldVertsB1.push_back(transB*b); - } - } - - - if (closestFaceB>=0) - clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); + worldVertsB1.resize(0); + { + const btFace& polyB = hullB.m_faces[closestFaceB]; + const int numVertices = polyB.m_indices.size(); + for (int e0 = 0; e0 < numVertices; e0++) + { + const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; + worldVertsB1.push_back(transB * b); + } + } + if (closestFaceB >= 0) + clipFaceAgainstHull(separatingNormal, hullA, transA, worldVertsB1, worldVertsB2, minDist, maxDist, resultOut); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h index b87bd4f3245..328f6424bc8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -13,14 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans - #ifndef BT_POLYHEDRAL_CONTACT_CLIPPING_H #define BT_POLYHEDRAL_CONTACT_CLIPPING_H - #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btTransform.h" #include "btDiscreteCollisionDetectorInterface.h" @@ -32,15 +29,14 @@ typedef btAlignedObjectArray<btVector3> btVertexArray; // Clips a face to the back of a plane struct btPolyhedralContactClipping { - static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); - static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); + static void clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut); - static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); + static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); - ///the clipFace method is used internally - static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS); + static bool findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); + ///the clipFace method is used internally + static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS); }; -#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H - +#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 786efd18200..3d11e5bce5f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -23,39 +23,38 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "btRaycastCallback.h" -btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags) - : - m_from(from), - m_to(to), - //@BP Mod - m_flags(flags), - m_hitFraction(btScalar(1.)) +btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags) + : m_from(from), + m_to(to), + //@BP Mod + m_flags(flags), + m_hitFraction(btScalar(1.)) { - } - - -void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +void btTriangleRaycastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - const btVector3 &vert0=triangle[0]; - const btVector3 &vert1=triangle[1]; - const btVector3 &vert2=triangle[2]; + const btVector3& vert0 = triangle[0]; + const btVector3& vert1 = triangle[1]; + const btVector3& vert2 = triangle[2]; + + btVector3 v10; + v10 = vert1 - vert0; + btVector3 v20; + v20 = vert2 - vert0; - btVector3 v10; v10 = vert1 - vert0 ; - btVector3 v20; v20 = vert2 - vert0 ; + btVector3 triangleNormal; + triangleNormal = v10.cross(v20); - btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); - const btScalar dist = vert0.dot(triangleNormal); - btScalar dist_a = triangleNormal.dot(m_from) ; - dist_a-= dist; + btScalar dist_a = triangleNormal.dot(m_from); + dist_a -= dist; btScalar dist_b = triangleNormal.dot(m_to); dist_b -= dist; - if ( dist_a * dist_b >= btScalar(0.0) ) + if (dist_a * dist_b >= btScalar(0.0)) { - return ; // same sign + return; // same sign } if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0))) @@ -64,52 +63,52 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, return; } - - const btScalar proj_length=dist_a-dist_b; - const btScalar distance = (dist_a)/(proj_length); + const btScalar proj_length = dist_a - dist_b; + const btScalar distance = (dist_a) / (proj_length); // Now we have the intersection point on the plane, we'll see if it's inside the triangle // Add an epsilon as a tolerance for the raycast, // in case the ray hits exacly on the edge of the triangle. // It must be scaled for the triangle size. - - if(distance < m_hitFraction) - { - - btScalar edge_tolerance =triangleNormal.length2(); + if (distance < m_hitFraction) + { + btScalar edge_tolerance = triangleNormal.length2(); edge_tolerance *= btScalar(-0.0001); - btVector3 point; point.setInterpolate3( m_from, m_to, distance); + btVector3 point; + point.setInterpolate3(m_from, m_to, distance); { - btVector3 v0p; v0p = vert0 - point; - btVector3 v1p; v1p = vert1 - point; - btVector3 cp0; cp0 = v0p.cross( v1p ); - - if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) + btVector3 v0p; + v0p = vert0 - point; + btVector3 v1p; + v1p = vert1 - point; + btVector3 cp0; + cp0 = v0p.cross(v1p); + + if ((btScalar)(cp0.dot(triangleNormal)) >= edge_tolerance) { - - - btVector3 v2p; v2p = vert2 - point; + btVector3 v2p; + v2p = vert2 - point; btVector3 cp1; - cp1 = v1p.cross( v2p); - if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) + cp1 = v1p.cross(v2p); + if ((btScalar)(cp1.dot(triangleNormal)) >= edge_tolerance) { btVector3 cp2; cp2 = v2p.cross(v0p); - - if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) + + if ((btScalar)(cp2.dot(triangleNormal)) >= edge_tolerance) { - //@BP Mod - // Triangle normal isn't normalized - triangleNormal.normalize(); + //@BP Mod + // Triangle normal isn't normalized + triangleNormal.normalize(); - //@BP Mod - Allow for unflipped normal when raycasting against backfaces + //@BP Mod - Allow for unflipped normal when raycasting against backfaces if (((m_flags & kF_KeepUnflippedNormal) == 0) && (dist_a <= btScalar(0.0))) { - m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(-triangleNormal, distance, partId, triangleIndex); } else { - m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex); } } } @@ -118,8 +117,7 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, } } - -btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) +btTriangleConvexcastCallback::btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) { m_convexShape = convexShape; m_convexShapeFrom = convexShapeFrom; @@ -130,14 +128,13 @@ btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* m_allowedPenetration = 0.f; } -void -btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) +void btTriangleConvexcastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); - triangleShape.setMargin(m_triangleCollisionMargin); + btTriangleShape triangleShape(triangle[0], triangle[1], triangle[2]); + triangleShape.setMargin(m_triangleCollisionMargin); - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; //#define USE_SUBSIMPLEX_CONVEX_CAST 1 //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below @@ -145,21 +142,21 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, 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 - + btContinuousConvexCollision convexCaster(m_convexShape, &triangleShape, &simplexSolver, &gjkEpaPenetrationSolver); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + btConvexCast::CastResult castResult; castResult.m_fraction = btScalar(1.); castResult.m_allowedPenetration = m_allowedPenetration; - if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) + 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 */ -/* + /* 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; @@ -167,11 +164,11 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, */ castResult.m_normal.normalize(); - reportHit (castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction, - partId, - triangleIndex); + 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 f2ed0cd39c4..2d0df718a23 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -21,35 +21,34 @@ subject to the following restrictions: struct btBroadphaseProxy; class btConvexShape; -class btTriangleRaycastCallback: public btTriangleCallback +class btTriangleRaycastCallback : public btTriangleCallback { public: - //input btVector3 m_from; btVector3 m_to; - //@BP Mod - allow backface filtering and unflipped normals - enum EFlags - { - kF_None = 0, - kF_FilterBackfaces = 1 << 0, - kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle - ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. - kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm - kF_UseGjkConvexCastRaytest = 1 << 3, - kF_Terminator = 0xFFFFFFFF - }; - unsigned int m_flags; + //@BP Mod - allow backface filtering and unflipped normals + enum EFlags + { + kF_None = 0, + kF_FilterBackfaces = 1 << 0, + kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle + ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. + kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm + kF_UseGjkConvexCastRaytest = 1 << 3, + kF_DisableHeightfieldAccelerator = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062 + kF_Terminator = 0xFFFFFFFF + }; + unsigned int m_flags; + + btScalar m_hitFraction; - btScalar m_hitFraction; + btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags = 0); - btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0); - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0; - + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; }; class btTriangleConvexcastCallback : public btTriangleCallback @@ -63,12 +62,11 @@ public: btScalar m_triangleCollisionMargin; btScalar m_allowedPenetration; - btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar 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 void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; }; -#endif //BT_RAYCAST_TRI_CALLBACK_H - +#endif //BT_RAYCAST_TRI_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index da8a13914c9..ccd227109d2 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_SIMPLEX_SOLVER_INTERFACE_H #define BT_SIMPLEX_SOLVER_INTERFACE_H @@ -31,33 +29,30 @@ subject to the following restrictions: /// voronoi regions or barycentric coordinates class btSimplexSolverInterface { - public: - virtual ~btSimplexSolverInterface() {}; +public: + virtual ~btSimplexSolverInterface(){}; virtual void reset() = 0; virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0; - + virtual bool closest(btVector3& v) = 0; virtual btScalar maxVertex() = 0; virtual bool fullSimplex() const = 0; - virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0; + virtual int getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const = 0; virtual bool inSimplex(const btVector3& w) = 0; - + virtual void backup_closest(btVector3& v) = 0; virtual bool emptySimplex() const = 0; virtual void compute_points(btVector3& p1, btVector3& p2) = 0; - virtual int numVertices() const =0; - - + virtual int numVertices() const = 0; }; #endif -#endif //BT_SIMPLEX_SOLVER_INTERFACE_H - +#endif //BT_SIMPLEX_SOLVER_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index ec638f60ba5..37458339e70 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSubSimplexConvexCast.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" @@ -22,32 +21,26 @@ subject to the following restrictions: #include "btPointCollector.h" #include "LinearMath/btTransformUtil.h" -btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) -:m_simplexSolver(simplexSolver), -m_convexA(convexA),m_convexB(convexB) +btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver) + : m_simplexSolver(simplexSolver), + m_convexA(convexA), + m_convexB(convexB) { } -///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. -///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 -#ifdef BT_USE_DOUBLE_PRECISION -#define MAX_ITERATIONS 64 -#else -#define MAX_ITERATIONS 32 -#endif -bool btSubsimplexConvexCast::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) -{ +bool btSubsimplexConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) +{ m_simplexSolver->reset(); - btVector3 linVelA,linVelB; - linVelA = toA.getOrigin()-fromA.getOrigin(); - linVelB = toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = toA.getOrigin() - fromA.getOrigin(); + linVelB = toB.getOrigin() - fromB.getOrigin(); btScalar lambda = btScalar(0.); @@ -55,36 +48,31 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( btTransform interpolatedTransB = fromB; ///take relative motion - btVector3 r = (linVelA-linVelB); + btVector3 r = (linVelA - linVelB); btVector3 v; - - 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 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r * fromA.getBasis())); + btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r * fromB.getBasis())); + v = supVertexA - supVertexB; + int maxIter = result.m_subSimplexCastMaxIterations; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 c; - + btScalar dist2 = v.length2(); - btScalar dist2 = v.length2(); -#ifdef BT_USE_DOUBLE_PRECISION - btScalar epsilon = btScalar(0.0001); -#else - btScalar epsilon = btScalar(0.0001); -#endif //BT_USE_DOUBLE_PRECISION - btVector3 w,p; + + btVector3 w, p; btScalar VdotR; - - while ( (dist2 > epsilon) && maxIter--) + + while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--) { - supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); - supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); - w = supVertexA-supVertexB; + supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis())); + supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis())); + w = supVertexA - supVertexB; btScalar VdotW = v.dot(w); @@ -93,68 +81,63 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( return false; } - if ( VdotW > btScalar(0.)) + if (VdotW > btScalar(0.)) { VdotR = v.dot(r); - if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) + if (VdotR >= -(SIMD_EPSILON * SIMD_EPSILON)) return false; else { lambda = lambda - VdotW / VdotR; //interpolate to next lambda // x = s + lambda * r; - interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); - interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); + interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda); + interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); //m_simplexSolver->reset(); //check next line - w = supVertexA-supVertexB; - + w = supVertexA - supVertexB; + n = v; - } - } + } ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc. if (!m_simplexSolver->inSimplex(w)) - m_simplexSolver->addVertex( w, supVertexA , supVertexB); + m_simplexSolver->addVertex(w, supVertexA, supVertexB); if (m_simplexSolver->closest(v)) { dist2 = v.length2(); - + //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()); - } else + } + else { dist2 = btScalar(0.); - } + } } //int numiter = MAX_ITERATIONS - maxIter; -// printf("number of iterations: %d", numiter); - + // printf("number of iterations: %d", numiter); + //don't report a time of impact when moving 'away' from the hitnormal - result.m_fraction = lambda; - if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON)) + 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) + if (result.m_normal.dot(r) >= -result.m_allowedPenetration) return false; - btVector3 hitA,hitB; - m_simplexSolver->compute_points(hitA,hitB); - result.m_hitPoint=hitB; + btVector3 hitA, hitB; + m_simplexSolver->compute_points(hitA, hitB); + result.m_hitPoint = hitB; return true; } - - - - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h index 6c8127983eb..0638a30eb1a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_SUBSIMPLEX_CONVEX_CAST_H #define BT_SUBSIMPLEX_CONVEX_CAST_H @@ -28,23 +27,21 @@ class btConvexShape; class btSubsimplexConvexCast : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_convexA; - const btConvexShape* m_convexB; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - - btSubsimplexConvexCast (const btConvexShape* shapeA,const btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver); + btSubsimplexConvexCast(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver); //virtual ~btSubsimplexConvexCast(); ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector. - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; -#endif //BT_SUBSIMPLEX_CONVEX_CAST_H +#endif //BT_SUBSIMPLEX_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 23b4f79cfc2..8fda94d2adf 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -23,26 +23,24 @@ subject to the following restrictions: */ - #include "btVoronoiSimplexSolver.h" -#define VERTA 0 -#define VERTB 1 -#define VERTC 2 -#define VERTD 3 +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 #define CATCH_DEGENERATE_TETRAHEDRON 1 -void btVoronoiSimplexSolver::removeVertex(int index) +void btVoronoiSimplexSolver::removeVertex(int index) { - - btAssert(m_numVertices>0); + btAssert(m_numVertices > 0); m_numVertices--; m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; } -void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) +void btVoronoiSimplexSolver::reduceVertices(const btUsageBitfield& usedVerts) { if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) removeVertex(3); @@ -52,29 +50,22 @@ void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) removeVertex(1); - + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) removeVertex(0); - } - - - - //clear the simplex, remove all the vertices void btVoronoiSimplexSolver::reset() { m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + m_lastW = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); m_cachedBC.reset(); } - - - //add a vertex +//add a vertex void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) { m_lastW = w; @@ -87,9 +78,8 @@ void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, c m_numVertices++; } -bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() +bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() { - if (m_needsUpdate) { m_cachedBC.reset(); @@ -98,127 +88,131 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() switch (numVertices()) { - case 0: + case 0: m_cachedValidClosest = false; break; - case 1: + case 1: { m_cachedP1 = m_simplexPointsP[0]; m_cachedP2 = m_simplexPointsQ[0]; - m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] + m_cachedV = m_cachedP1 - m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); + m_cachedBC.setBarycentricCoordinates(btScalar(1.), btScalar(0.), btScalar(0.), btScalar(0.)); m_cachedValidClosest = m_cachedBC.isValid(); break; }; - case 2: + case 2: { - //closest point origin from line segment - const btVector3& from = m_simplexVectorW[0]; - const btVector3& to = m_simplexVectorW[1]; - btVector3 nearest; - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 diff = p - from; - btVector3 v = to - from; - btScalar t = v.dot(diff); - - if (t > 0) { - btScalar dotVV = v.dot(v); - if (t < dotVV) { - t /= dotVV; - diff -= t*v; - m_cachedBC.m_usedVertices.usedVertexA = true; - m_cachedBC.m_usedVertices.usedVertexB = true; - } else { - t = 1; - diff -= v; - //reduce to 1 point - m_cachedBC.m_usedVertices.usedVertexB = true; - } - } else + //closest point origin from line segment + const btVector3& from = m_simplexVectorW[0]; + const btVector3& to = m_simplexVectorW[1]; + btVector3 nearest; + + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 diff = p - from; + btVector3 v = to - from; + btScalar t = v.dot(diff); + + if (t > 0) + { + btScalar dotVV = v.dot(v); + if (t < dotVV) { - t = 0; - //reduce to 1 point + t /= dotVV; + diff -= t * v; m_cachedBC.m_usedVertices.usedVertexA = true; + m_cachedBC.m_usedVertices.usedVertexB = true; + } + else + { + t = 1; + diff -= v; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexB = true; } - m_cachedBC.setBarycentricCoordinates(1-t,t); - nearest = from + t*v; + } + else + { + t = 0; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexA = true; + } + m_cachedBC.setBarycentricCoordinates(1 - t, t); + nearest = from + t * v; - m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); - m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); - m_cachedV = m_cachedP1 - m_cachedP2; - - reduceVertices(m_cachedBC.m_usedVertices); + m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); + m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); + m_cachedV = m_cachedP1 - m_cachedP2; - m_cachedValidClosest = m_cachedBC.isValid(); - break; + reduceVertices(m_cachedBC.m_usedVertices); + + m_cachedValidClosest = m_cachedBC.isValid(); + break; } - case 3: - { - //closest point origin from triangle - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); + case 3: + { + //closest point origin from triangle + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; - closestPtPointTriangle(p,a,b,c,m_cachedBC); - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; + closestPtPointTriangle(p, a, b, c, m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; - m_cachedV = m_cachedP1-m_cachedP2; + m_cachedV = m_cachedP1 - m_cachedP2; - reduceVertices (m_cachedBC.m_usedVertices); - m_cachedValidClosest = m_cachedBC.isValid(); + reduceVertices(m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); - break; + break; } - case 4: + case 4: { + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - const btVector3& a = m_simplexVectorW[0]; const btVector3& b = m_simplexVectorW[1]; const btVector3& c = m_simplexVectorW[2]; const btVector3& d = m_simplexVectorW[3]; - bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + bool hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC); - if (hasSeperation) + if (hasSeparation) { - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; - m_cachedV = m_cachedP1-m_cachedP2; - reduceVertices (m_cachedBC.m_usedVertices); - } else + m_cachedV = m_cachedP1 - m_cachedP2; + reduceVertices(m_cachedBC.m_usedVertices); + } + else { -// printf("sub distance got penetration\n"); + // printf("sub distance got penetration\n"); if (m_cachedBC.m_degenerate) { m_cachedValidClosest = false; - } else + } + else { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero - m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_cachedV.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } break; } @@ -228,7 +222,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() //closest point origin from tetrahedron break; } - default: + default: { m_cachedValidClosest = false; } @@ -236,7 +230,6 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() } return m_cachedValidClosest; - } //return/calculate the closest vertex @@ -247,13 +240,11 @@ bool btVoronoiSimplexSolver::closest(btVector3& v) return succes; } - - btScalar btVoronoiSimplexSolver::maxVertex() { int i, numverts = numVertices(); btScalar maxV = btScalar(0.); - for (i=0;i<numverts;i++) + for (i = 0; i < numverts; i++) { btScalar curLen2 = m_simplexVectorW[i].length2(); if (maxV < curLen2) @@ -262,13 +253,11 @@ btScalar btVoronoiSimplexSolver::maxVertex() return maxV; } - - - //return the current simplex -int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const +//return the current simplex +int btVoronoiSimplexSolver::getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const { int i; - for (i=0;i<numVertices();i++) + for (i = 0; i < numVertices(); i++) { yBuf[i] = m_simplexVectorW[i]; pBuf[i] = m_simplexPointsP[i]; @@ -277,20 +266,17 @@ int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVecto return numVertices(); } - - - bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) { bool found = false; int i, numverts = numVertices(); //btScalar maxV = btScalar(0.); - + //w is in the current (reduced) simplex - for (i=0;i<numverts;i++) + for (i = 0; i < numverts; i++) { #ifdef BT_USE_EQUAL_VERTEX_THRESHOLD - if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) + if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) #else if (m_simplexVectorW[i] == w) #endif @@ -303,199 +289,190 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) //check in case lastW is already removed if (w == m_lastW) return true; - + return found; } -void btVoronoiSimplexSolver::backup_closest(btVector3& v) +void btVoronoiSimplexSolver::backup_closest(btVector3& v) { v = m_cachedV; } - -bool btVoronoiSimplexSolver::emptySimplex() const +bool btVoronoiSimplexSolver::emptySimplex() const { return (numVertices() == 0); - } -void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) +void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) { updateClosestVectorAndPoints(); p1 = m_cachedP1; p2 = m_cachedP2; - } - - - -bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result) +bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result) { result.m_usedVertices.reset(); - // Check if P in vertex region outside A - btVector3 ab = b - a; - btVector3 ac = c - a; - btVector3 ap = p - a; - btScalar d1 = ab.dot(ap); - btScalar d2 = ac.dot(ap); - if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) + // Check if P in vertex region outside A + btVector3 ab = b - a; + btVector3 ac = c - a; + btVector3 ap = p - a; + btScalar d1 = ab.dot(ap); + btScalar d2 = ac.dot(ap); + if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) { result.m_closestPointOnSimplex = a; result.m_usedVertices.usedVertexA = true; - result.setBarycentricCoordinates(1,0,0); - return true;// a; // barycentric coordinates (1,0,0) + result.setBarycentricCoordinates(1, 0, 0); + return true; // a; // barycentric coordinates (1,0,0) } - // Check if P in vertex region outside B - btVector3 bp = p - b; - btScalar d3 = ab.dot(bp); - btScalar d4 = ac.dot(bp); - if (d3 >= btScalar(0.0) && d4 <= d3) + // Check if P in vertex region outside B + btVector3 bp = p - b; + btScalar d3 = ab.dot(bp); + btScalar d4 = ac.dot(bp); + if (d3 >= btScalar(0.0) && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(0,1,0); + result.setBarycentricCoordinates(0, 1, 0); - return true; // b; // barycentric coordinates (0,1,0) + return true; // b; // barycentric coordinates (0,1,0) } - // Check if P in edge region of AB, if so return projection of P onto AB - btScalar vc = d1*d4 - d3*d2; - if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { - btScalar v = d1 / (d1 - d3); + // Check if P in edge region of AB, if so return projection of P onto AB + btScalar vc = d1 * d4 - d3 * d2; + if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) + { + btScalar v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(1-v,v,0); + result.setBarycentricCoordinates(1 - v, v, 0); return true; - //return a + v * ab; // barycentric coordinates (1-v,v,0) - } - - // Check if P in vertex region outside C - btVector3 cp = p - c; - btScalar d5 = ab.dot(cp); - btScalar d6 = ac.dot(cp); - if (d6 >= btScalar(0.0) && d5 <= d6) + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + btVector3 cp = p - c; + btScalar d5 = ab.dot(cp); + btScalar d6 = ac.dot(cp); + if (d6 >= btScalar(0.0) && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,0,1); - return true;//c; // barycentric coordinates (0,0,1) + result.setBarycentricCoordinates(0, 0, 1); + return true; //c; // barycentric coordinates (0,0,1) } - // Check if P in edge region of AC, if so return projection of P onto AC - btScalar vb = d5*d2 - d1*d6; - if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { - btScalar w = d2 / (d2 - d6); + // Check if P in edge region of AC, if so return projection of P onto AC + btScalar vb = d5 * d2 - d1 * d6; + if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) + { + btScalar w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-w,0,w); + result.setBarycentricCoordinates(1 - w, 0, w); return true; - //return a + w * ac; // barycentric coordinates (1-w,0,w) - } + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + btScalar va = d3 * d6 - d5 * d4; + if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) + { + btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - // Check if P in edge region of BC, if so return projection of P onto BC - btScalar va = d3*d6 - d5*d4; - if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { - btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,1-w,w); - return true; - // return b + w * (c - b); // barycentric coordinates (0,1-w,w) - } - - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - btScalar denom = btScalar(1.0) / (va + vb + vc); - btScalar v = vb * denom; - btScalar w = vc * denom; - + result.setBarycentricCoordinates(0, 1 - w, w); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + btScalar denom = btScalar(1.0) / (va + vb + vc); + btScalar v = vb * denom; + btScalar w = vc * denom; + result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-v-w,v,w); - - return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w + result.setBarycentricCoordinates(1 - v - w, v, w); + return true; + // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w } - - - - /// Test if point p and d lie on opposite sides of plane through abc int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d) { - btVector3 normal = (b-a).cross(c-a); + btVector3 normal = (b - a).cross(c - a); - btScalar signp = (p - a).dot(normal); // [AP AB AC] - btScalar signd = (d - a).dot( normal); // [AD AB AC] + btScalar signp = (p - a).dot(normal); // [AP AB AC] + btScalar signd = (d - a).dot(normal); // [AD AB AC] #ifdef CATCH_DEGENERATE_TETRAHEDRON #ifdef BT_USE_DOUBLE_PRECISION -if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) { return -1; } #else if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) { -// printf("affine dependent/degenerate\n");// + // printf("affine dependent/degenerate\n");// return -1; } #endif #endif // Points on opposite sides if expression signs are opposite - return signp * signd < btScalar(0.); + return signp * signd < btScalar(0.); } - -bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) +bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) { btSubSimplexClosestResult tempResult; - // Start out assuming point inside all halfspaces, so closest to itself + // Start out assuming point inside all halfspaces, so closest to itself finalResult.m_closestPointOnSimplex = p; finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = true; + finalResult.m_usedVertices.usedVertexA = true; finalResult.m_usedVertices.usedVertexB = true; finalResult.m_usedVertices.usedVertexC = true; finalResult.m_usedVertices.usedVertexD = true; - int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); + int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); - int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); - int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); - - if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) - { - finalResult.m_degenerate = true; - return false; - } + int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); - if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) - { - return false; - } + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.m_degenerate = true; + return false; + } + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } - btScalar bestSqDist = FLT_MAX; - // If point outside face abc then compute closest point on abc - if (pointOutsideABC) + btScalar bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) { - closestPtPointTriangle(p, a, b, c,tempResult); + closestPtPointTriangle(p, a, b, c, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; - - btScalar sqDist = (q - p).dot( q - p); - // Update best closest point if (squared) distance is less than current best - if (sqDist < bestSqDist) { + + btScalar sqDist = (q - p).dot(q - p); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) + { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! @@ -504,25 +481,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC], - 0 - ); - + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC], + 0); } - } - + } // Repeat test for face acd - if (pointOutsideACD) + if (pointOutsideACD) { - closestPtPointTriangle(p, a, c, d,tempResult); + closestPtPointTriangle(p, a, c, d, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; @@ -532,52 +506,46 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - 0, - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC] - ); - + tempResult.m_barycentricCoords[VERTA], + 0, + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC]); } - } - // Repeat test for face adb + } + // Repeat test for face adb - if (pointOutsideADB) { - closestPtPointTriangle(p, a, d, b,tempResult); + closestPtPointTriangle(p, a, d, b, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; - + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - 0, - tempResult.m_barycentricCoords[VERTB] - ); - + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + 0, + tempResult.m_barycentricCoords[VERTB]); } - } - // Repeat test for face bdc - + } + // Repeat test for face bdc if (pointOutsideBDC) { - closestPtPointTriangle(p, b, d, c,tempResult); + closestPtPointTriangle(p, b, d, c, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; @@ -588,25 +556,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( - 0, - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - tempResult.m_barycentricCoords[VERTB] - ); - + 0, + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + tempResult.m_barycentricCoords[VERTB]); } - } + } //help! we ended up full ! - + if (finalResult.m_usedVertices.usedVertexA && finalResult.m_usedVertices.usedVertexB && finalResult.m_usedVertices.usedVertexC && - finalResult.m_usedVertices.usedVertexD) + finalResult.m_usedVertices.usedVertexD) { return true; } - return true; + return true; } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 2f389e27e3f..24a0a8f2df8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -13,23 +13,24 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_VORONOI_SIMPLEX_SOLVER_H #define BT_VORONOI_SIMPLEX_SOLVER_H #include "btSimplexSolverInterface.h" - - #define VORONOI_SIMPLEX_MAX_VERTS 5 ///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure #define BT_USE_EQUAL_VERTEX_THRESHOLD -#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f +#ifdef BT_USE_DOUBLE_PRECISION +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f +#else +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f +#endif //BT_USE_DOUBLE_PRECISION -struct btUsageBitfield{ +struct btUsageBitfield +{ btUsageBitfield() { reset(); @@ -42,140 +43,131 @@ struct btUsageBitfield{ usedVertexC = false; usedVertexD = false; } - unsigned short usedVertexA : 1; - unsigned short usedVertexB : 1; - unsigned short usedVertexC : 1; - unsigned short usedVertexD : 1; - unsigned short unused1 : 1; - unsigned short unused2 : 1; - unsigned short unused3 : 1; - unsigned short unused4 : 1; + unsigned short usedVertexA : 1; + unsigned short usedVertexB : 1; + unsigned short usedVertexC : 1; + unsigned short usedVertexD : 1; + unsigned short unused1 : 1; + unsigned short unused2 : 1; + unsigned short unused3 : 1; + unsigned short unused4 : 1; }; - -struct btSubSimplexClosestResult +struct btSubSimplexClosestResult { - btVector3 m_closestPointOnSimplex; + btVector3 m_closestPointOnSimplex; //MASK for m_usedVertices - //stores the simplex vertex-usage, using the MASK, + //stores the simplex vertex-usage, using the MASK, // if m_usedVertices & MASK then the related vertex is used - btUsageBitfield m_usedVertices; - btScalar m_barycentricCoords[4]; + btUsageBitfield m_usedVertices; + btScalar m_barycentricCoords[4]; bool m_degenerate; - void reset() + void reset() { m_degenerate = false; setBarycentricCoordinates(); m_usedVertices.reset(); } - bool isValid() + bool isValid() { bool valid = (m_barycentricCoords[0] >= btScalar(0.)) && - (m_barycentricCoords[1] >= btScalar(0.)) && - (m_barycentricCoords[2] >= btScalar(0.)) && - (m_barycentricCoords[3] >= btScalar(0.)); - + (m_barycentricCoords[1] >= btScalar(0.)) && + (m_barycentricCoords[2] >= btScalar(0.)) && + (m_barycentricCoords[3] >= btScalar(0.)); return valid; } - void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.)) + void setBarycentricCoordinates(btScalar a = btScalar(0.), btScalar b = btScalar(0.), btScalar c = btScalar(0.), btScalar d = btScalar(0.)) { m_barycentricCoords[0] = a; m_barycentricCoords[1] = b; m_barycentricCoords[2] = c; m_barycentricCoords[3] = d; } - }; /// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. /// Can be used with GJK, as an alternative to Johnson distance algorithm. #ifdef NO_VIRTUAL_INTERFACE -ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver +ATTRIBUTE_ALIGNED16(class) +btVoronoiSimplexSolver #else -ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface +ATTRIBUTE_ALIGNED16(class) +btVoronoiSimplexSolver : public btSimplexSolverInterface #endif { public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_numVertices; + int m_numVertices; - btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; - - - btVector3 m_cachedP1; - btVector3 m_cachedP2; - btVector3 m_cachedV; - btVector3 m_lastW; - - btScalar m_equalVertexThreshold; - bool m_cachedValidClosest; + btVector3 m_cachedP1; + btVector3 m_cachedP2; + btVector3 m_cachedV; + btVector3 m_lastW; + btScalar m_equalVertexThreshold; + bool m_cachedValidClosest; btSubSimplexClosestResult m_cachedBC; - bool m_needsUpdate; - - void removeVertex(int index); - void reduceVertices (const btUsageBitfield& usedVerts); - bool updateClosestVectorAndPoints(); + bool m_needsUpdate; - bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); - int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); - bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result); + void removeVertex(int index); + void reduceVertices(const btUsageBitfield& usedVerts); + bool updateClosestVectorAndPoints(); -public: + bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); + int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); + bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result); +public: btVoronoiSimplexSolver() - : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) + : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) { } - void reset(); - - void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); + void reset(); - void setEqualVertexThreshold(btScalar threshold) - { - m_equalVertexThreshold = threshold; - } + void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); - btScalar getEqualVertexThreshold() const - { - return m_equalVertexThreshold; - } + void setEqualVertexThreshold(btScalar threshold) + { + m_equalVertexThreshold = threshold; + } - bool closest(btVector3& v); + btScalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } - btScalar maxVertex(); + bool closest(btVector3 & v); - bool fullSimplex() const - { - return (m_numVertices == 4); - } + btScalar maxVertex(); - int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; + bool fullSimplex() const + { + return (m_numVertices == 4); + } - bool inSimplex(const btVector3& w); - - void backup_closest(btVector3& v) ; + int getSimplex(btVector3 * pBuf, btVector3 * qBuf, btVector3 * yBuf) const; - bool emptySimplex() const ; + bool inSimplex(const btVector3& w); - void compute_points(btVector3& p1, btVector3& p2) ; + void backup_closest(btVector3 & v); - int numVertices() const - { - return m_numVertices; - } + bool emptySimplex() const; + void compute_points(btVector3 & p1, btVector3 & p2); + int numVertices() const + { + return m_numVertices; + } }; -#endif //BT_VORONOI_SIMPLEX_SOLVER_H - +#endif //BT_VORONOI_SIMPLEX_SOLVER_H |