diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision')
162 files changed, 6408 insertions, 3800 deletions
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index d7eea33ea41..77763305b1b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -19,10 +19,9 @@ // 3. This notice may not be removed or altered from any source distribution. #include "btAxisSweep3.h" -#include <assert.h> -btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache) -:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache) +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); @@ -30,8 +29,8 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab } -bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache ) -:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache) +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 d0ad09a385a..cad21b4cad2 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -19,12 +19,12 @@ #ifndef AXIS_SWEEP_3_H #define AXIS_SWEEP_3_H -#include "LinearMath/btPoint3.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 @@ -42,6 +42,7 @@ protected: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); class Edge { @@ -61,8 +62,7 @@ public: // indexes into the edge arrays BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 // BP_FP_INT_TYPE m_uniqueId; - BP_FP_INT_TYPE m_pad; - + 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;} @@ -71,8 +71,8 @@ public: protected: - btPoint3 m_worldAabbMin; // overall system bounds - btPoint3 m_worldAabbMax; // overall system bounds + btVector3 m_worldAabbMin; // overall system bounds + btVector3 m_worldAabbMax; // overall system bounds btVector3 m_quantize; // scaling factor for quantization @@ -94,6 +94,12 @@ protected: 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); @@ -108,7 +114,7 @@ protected: //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 quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const; + 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 ); @@ -117,7 +123,7 @@ protected: public: - btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0); + 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(); @@ -128,17 +134,26 @@ public: virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + 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 btPoint3& aabbMin,const btPoint3& aabbMax,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)); + + 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); @@ -206,7 +221,7 @@ void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinalit } if (checkCardinality) - assert(numEdges == m_numHandles*2+1); + btAssert(numEdges == m_numHandles*2+1); } #endif //DEBUG_BROADPHASE @@ -217,7 +232,12 @@ btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy( const btV 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; } @@ -227,6 +247,8 @@ 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); } @@ -234,22 +256,80 @@ 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>::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 btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache ) +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_invalidPair(0), +m_raycastAccelerator(0) { BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle @@ -260,7 +340,14 @@ m_invalidPair(0) m_ownsPairCache = true; } - //assert(bounds.HasVolume()); + 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; @@ -320,7 +407,14 @@ m_invalidPair(0) 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]); @@ -335,27 +429,31 @@ btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal() } template <typename BP_FP_INT_TYPE> -void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const +void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const { - btPoint3 clampedPoint(point); - - - +#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() { - assert(m_firstFreeHandle); + btAssert(m_firstFreeHandle); BP_FP_INT_TYPE handle = m_firstFreeHandle; m_firstFreeHandle = getHandle(handle)->GetNextFree(); @@ -367,7 +465,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle() template <typename BP_FP_INT_TYPE> void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle) { - assert(handle > 0 && handle < m_maxHandles); + btAssert(handle > 0 && handle < m_maxHandles); getHandle(handle)->SetNextFree(m_firstFreeHandle); m_firstFreeHandle = handle; @@ -377,7 +475,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle) template <typename BP_FP_INT_TYPE> -BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) +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]; @@ -440,7 +538,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,bt //explicitly remove the pairs containing the proxy //we could do it also in the sortMinUp (passing true) - //todo: compare performance + ///@todo: compare performance if (!m_pairCache->hasDeferredRemoval()) { m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); @@ -489,6 +587,21 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,bt } +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> @@ -529,6 +642,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche 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) @@ -574,10 +688,6 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); } - - - - } @@ -616,10 +726,10 @@ bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, } template <typename BP_FP_INT_TYPE> -void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher) +void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) { -// assert(bounds.IsFinite()); - //assert(bounds.HasVolume()); +// btAssert(bounds.IsFinite()); + //btAssert(bounds.HasVolume()); Handle* pHandle = getHandle(handle); @@ -895,7 +1005,7 @@ class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int> { public: - btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0); + btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); }; @@ -906,7 +1016,7 @@ class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int> { public: - bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0); + bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index 200ac365329..b7bbaf512ae 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -21,8 +21,22 @@ subject to the following restrictions: struct btDispatcherInfo; class btDispatcher; #include "btBroadphaseProxy.h" + class btOverlappingPairCache; + + +struct btBroadphaseRayCallback +{ + ///added some cached data to accelerate ray-AABB tests + btVector3 m_rayDirectionInverse; + unsigned int m_signs[3]; + btScalar m_lambda_max; + + virtual ~btBroadphaseRayCallback() {} + virtual bool process(const btBroadphaseProxy* proxy) = 0; +}; + #include "LinearMath/btVector3.h" ///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs. @@ -36,7 +50,10 @@ public: 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 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; + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; @@ -47,6 +64,9 @@ public: ///will add some transform later virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0; + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher) {}; + virtual void printStats() = 0; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index a074a0b150b..be261ec4080 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -17,20 +17,24 @@ subject to the following restrictions: #define BROADPHASE_PROXY_H #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 +/// CUSTOM_POLYHEDRAL_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE and CUSTOM_CONCAVE_SHAPE_TYPE can be used to extend Bullet without modifying source code enum BroadphaseNativeTypes { -// polyhedral convex shapes + // polyhedral convex shapes BOX_SHAPE_PROXYTYPE, TRIANGLE_SHAPE_PROXYTYPE, TETRAHEDRAL_SHAPE_PROXYTYPE, CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE, CONVEX_HULL_SHAPE_PROXYTYPE, + CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE, + CUSTOM_POLYHEDRAL_SHAPE_TYPE, //implicit convex shapes IMPLICIT_CONVEX_SHAPES_START_HERE, SPHERE_SHAPE_PROXYTYPE, @@ -42,6 +46,7 @@ IMPLICIT_CONVEX_SHAPES_START_HERE, UNIFORM_SCALING_SHAPE_PROXYTYPE, MINKOWSKI_SUM_SHAPE_PROXYTYPE, MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, + CUSTOM_CONVEX_SHAPE_TYPE, //concave shapes CONCAVE_SHAPES_START_HERE, //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! @@ -58,13 +63,18 @@ CONCAVE_SHAPES_START_HERE, EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, + CUSTOM_CONCAVE_SHAPE_TYPE, CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, SOFTBODY_SHAPE_PROXYTYPE, + HFFLUID_SHAPE_PROXYTYPE, + HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE, + INVALID_SHAPE_PROXYTYPE, MAX_BROADPHASE_COLLISION_TYPES + }; @@ -83,20 +93,20 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); 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. + btVector3 m_aabbMin; + btVector3 m_aabbMax; + SIMD_FORCE_INLINE int getUid() const { return m_uniqueId; @@ -107,10 +117,12 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { } - btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=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_collisionFilterMask(collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) { m_multiSapParentProxy = multiSapParentProxy; } @@ -159,7 +171,7 @@ ATTRIBUTE_ALIGNED16(struct) btBroadphasePair m_pProxy0(0), m_pProxy1(0), m_algorithm(0), - m_userInfo(0) + m_internalInfo1(0) { } @@ -169,14 +181,14 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); : m_pProxy0(other.m_pProxy0), m_pProxy1(other.m_pProxy1), m_algorithm(other.m_algorithm), - m_userInfo(other.m_userInfo) + m_internalInfo1(other.m_internalInfo1) { } btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) { //keep them sorted, so the std::set operations work - if (&proxy0 < &proxy1) + if (proxy0.m_uniqueId < proxy1.m_uniqueId) { m_pProxy0 = &proxy0; m_pProxy1 = &proxy1; @@ -188,7 +200,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); } m_algorithm = 0; - m_userInfo = 0; + m_internalInfo1 = 0; } @@ -196,7 +208,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); btBroadphaseProxy* m_pProxy1; mutable btCollisionAlgorithm* m_algorithm; - mutable void* m_userInfo; + union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version. }; @@ -217,8 +229,13 @@ class btBroadphasePairSortPredicate bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) { - return a.m_pProxy0 > b.m_pProxy0 || - (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) || + 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); } }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 7c41c8d8f71..a6e36b47049 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -23,188 +23,188 @@ typedef btAlignedObjectArray<const btDbvtNode*> tConstNodeArray; // struct btDbvtNodeEnumerator : btDbvt::ICollide { -tConstNodeArray nodes; -void Process(const btDbvtNode* n) { nodes.push_back(n); } + tConstNodeArray nodes; + void Process(const btDbvtNode* n) { nodes.push_back(n); } }; // static DBVT_INLINE int indexof(const btDbvtNode* node) { -return(node->parent->childs[1]==node); + return(node->parent->childs[1]==node); } // static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, - const btDbvtVolume& b) + const btDbvtVolume& b) { -#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE -DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)]; -btDbvtVolume& res=*(btDbvtVolume*)locals; +#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); + btDbvtVolume& res=*(btDbvtVolume*)locals; #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) { -const btVector3 edges=a.Lengths(); -return( 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) { -if(node->isinternal()) + if(node->isinternal()) { - getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[0],depth+1,maxdepth); } else maxdepth=btMax(maxdepth,depth); } // static DBVT_INLINE void deletenode( btDbvt* pdbvt, - btDbvtNode* node) + btDbvtNode* node) { -btAlignedFree(pdbvt->m_free); -pdbvt->m_free=node; + btAlignedFree(pdbvt->m_free); + pdbvt->m_free=node; } - + // static void recursedeletenode( btDbvt* pdbvt, - btDbvtNode* node) + btDbvtNode* node) { -if(!node->isleaf()) + 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) + btDbvtNode* parent, + void* data) { -btDbvtNode* node; -if(pdbvt->m_free) + 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->parent = parent; + node->data = data; + node->childs[1] = 0; + return(node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume, - void* data) + btDbvtNode* 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) + 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) + 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)]; + 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; + break; + node=prev; } while(0!=(prev=node->parent)); } else { - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - pdbvt->m_root = node; + 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) + 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; + 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); } } } @@ -215,33 +215,33 @@ static void fetchleaves(btDbvt* pdbvt, 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 { - leaves.push_back(root); + leaves.push_back(root); } } // static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, - const btVector3& org, - const btVector3& axis) + tNodeArray& left, + tNodeArray& right, + const btVector3& org, + const btVector3& axis) { -left.resize(0); -right.resize(0); -for(int i=0,ni=leaves.size();i<ni;++i) + left.resize(0); + right.resize(0); + for(int i=0,ni=leaves.size();i<ni;++i) { - if(dot(axis,leaves[i]->volume.Center()-org)<0) - left.push_back(leaves[i]); + if(dot(axis,leaves[i]->volume.Center()-org)<0) + left.push_back(leaves[i]); else - right.push_back(leaves[i]); + right.push_back(leaves[i]); } } @@ -249,49 +249,49 @@ for(int i=0,ni=leaves.size();i<ni;++i) static btDbvtVolume bounds( const tNodeArray& leaves) { #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE -DBVT_ALIGN char locals[sizeof(btDbvtVolume)]; -btDbvtVolume& volume=*(btDbvtVolume*)locals; -volume=leaves[0]->volume; + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); + btDbvtVolume& volume=*(btDbvtVolume*)locals; + 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=leaves.size();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) + tNodeArray& leaves) { -while(leaves.size()>1) + while(leaves.size()>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<leaves.size();++i) { - for(int j=i+1;j<leaves.size();++j) + for(int j=i+1;j<leaves.size();++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.swap(minidx[1],leaves.size()-1); + leaves.pop_back(); } } @@ -300,175 +300,181 @@ static btDbvtNode* topdown(btDbvt* pdbvt, tNodeArray& leaves, int bu_treshold) { -static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; -if(leaves.size()>1) + static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; + if(leaves.size()>1) { - if(leaves.size()>bu_treshold) + if(leaves.size()>bu_treshold) { - const btDbvtVolume vol=bounds(leaves); - const btVector3 org=vol.Center(); - tNodeArray sets[2]; - int bestaxis=-1; - int bestmidp=leaves.size(); - int splitcount[3][2]={{0,0},{0,0},{0,0}}; - int i; - for( i=0;i<leaves.size();++i) + const btDbvtVolume vol=bounds(leaves); + const btVector3 org=vol.Center(); + tNodeArray sets[2]; + int bestaxis=-1; + int bestmidp=leaves.size(); + int splitcount[3][2]={{0,0},{0,0},{0,0}}; + int i; + for( i=0;i<leaves.size();++i) { - const btVector3 x=leaves[i]->volume.Center()-org; - for(int j=0;j<3;++j) + const btVector3 x=leaves[i]->volume.Center()-org; + for(int j=0;j<3;++j) { - ++splitcount[j][dot(x,axis[j])>0?1:0]; + ++splitcount[j][dot(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]); + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); + split(leaves,sets[0],sets[1],org,axis[bestaxis]); } else { - sets[0].reserve(leaves.size()/2+1); - sets[1].reserve(leaves.size()/2); - for(int i=0,ni=leaves.size();i<ni;++i) + sets[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]); + sets[i&1].push_back(leaves[i]); } } - btDbvtNode* node=createnode(pdbvt,0,vol,0); - node->childs[0]=topdown(pdbvt,sets[0],bu_treshold); - node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); - node->childs[0]->parent=node; - node->childs[1]->parent=node; - return(node); + btDbvtNode* node=createnode(pdbvt,0,vol,0); + node->childs[0]=topdown(pdbvt,sets[0],bu_treshold); + node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); } else { - bottomup(pdbvt,leaves); - return(leaves[0]); + bottomup(pdbvt,leaves); + return(leaves[0]); } } -return(leaves[0]); + return(leaves[0]); } // static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) { -btDbvtNode* p=n->parent; -btAssert(n->isinternal()); -if(p>n) + btDbvtNode* p=n->parent; + btAssert(n->isinternal()); + if(p>n) { - const int i=indexof(n); - const int j=1-i; - btDbvtNode* s=p->childs[j]; - btDbvtNode* q=p->parent; - btAssert(n==p->childs[i]); - if(q) q->childs[indexof(p)]=n; else r=n; - s->parent=n; - p->parent=n; - n->parent=q; - p->childs[0]=n->childs[0]; - p->childs[1]=n->childs[1]; - n->childs[0]->parent=p; - n->childs[1]->parent=p; - n->childs[i]=p; - n->childs[j]=s; - btSwap(p->volume,n->volume); - return(p); + 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 static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) { -while(n&&(count--)) n=n->parent; -return(n); + while(n&&(count--)) n=n->parent; + return(n); } +#endif // // Api // // - btDbvt::btDbvt() +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; } // - btDbvt::~btDbvt() +btDbvt::~btDbvt() { -clear(); + clear(); } // void btDbvt::clear() { -if(m_root) recursedeletenode(this,m_root); -btAlignedFree(m_free); -m_free=0; + if(m_root) + recursedeletenode(this,m_root); + btAlignedFree(m_free); + m_free=0; + m_lkhd = -1; + m_stkStack.clear(); + m_opath = 0; + } // 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]; + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + bottomup(this,leaves); + m_root=leaves[0]; } } // void btDbvt::optimizeTopDown(int bu_treshold) { -if(m_root) + if(m_root) { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + m_root=topdown(this,leaves,bu_treshold); } } // void btDbvt::optimizeIncremental(int passes) { -if(passes<0) passes=m_leaves; -if(m_root&&(passes>0)) + 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; + update(node); + ++m_opath; } while(--passes); } } @@ -476,104 +482,104 @@ if(m_root&&(passes>0)) // btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) { -btDbvtNode* leaf=createnode(this,0,volume,data); -insertleaf(this,m_root,leaf); -++m_leaves; -return(leaf); + btDbvtNode* leaf=createnode(this,0,volume,data); + insertleaf(this,m_root,leaf); + ++m_leaves; + return(leaf); } // void btDbvt::update(btDbvtNode* leaf,int lookahead) { -btDbvtNode* root=removeleaf(this,leaf); -if(root) + 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; } -insertleaf(this,root,leaf); + insertleaf(this,root,leaf); } // -void btDbvt::update(btDbvtNode* leaf,const 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; } -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)); -volume.SignedExpand(velocity); -update(leaf,volume); -return(true); + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(btVector3(margin,margin,margin)); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity) +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity) { -if(leaf->volume.Contain(volume)) return(false); -volume.SignedExpand(velocity); -update(leaf,volume); -return(true); + if(leaf->volume.Contain(volume)) return(false); + volume.SignedExpand(velocity); + 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) { -removeleaf(this,leaf); -deletenode(this,leaf); ---m_leaves; + removeleaf(this,leaf); + deletenode(this,leaf); + --m_leaves; } // void btDbvt::write(IWriter* iwriter) const { -btDbvtNodeEnumerator nodes; -nodes.nodes.reserve(m_leaves*2); -enumNodes(m_root,nodes); -iwriter->Prepare(m_root,nodes.nodes.size()); -for(int i=0;i<nodes.nodes.size();++i) + 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); } } } @@ -581,29 +587,29 @@ for(int i=0;i<nodes.nodes.size();++i) // void btDbvt::clone(btDbvt& dest,IClone* iclone) const { -dest.clear(); -if(m_root!=0) + dest.clear(); + if(m_root!=0) { - btAlignedObjectArray<sStkCLN> stack; - stack.reserve(m_leaves); - stack.push_back(sStkCLN(m_root,0)); - do { - const int i=stack.size()-1; - const sStkCLN e=stack[i]; - btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); - stack.pop_back(); - if(e.parent!=0) - e.parent->childs[i&1]=n; + btAlignedObjectArray<sStkCLN> stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); + stack.pop_back(); + if(e.parent!=0) + e.parent->childs[i&1]=n; else - dest.m_root=n; - if(e.node->isinternal()) + 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); + iclone->CloneLeaf(n); } } while(stack.size()>0); } @@ -612,31 +618,31 @@ if(m_root!=0) // 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) { -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) { -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); + leaves.push_back(node); } } @@ -657,19 +663,19 @@ q6600,2.4ghz /W3 /nologo /c /Wp64 /Zi /errorReport:prompt Benchmarking dbvt... - World scale: 100.000000 - Extents base: 1.000000 - Extents range: 4.000000 - Leaves: 8192 - sizeof(btDbvtVolume): 32 bytes - sizeof(btDbvtNode): 44 bytes +World scale: 100.000000 +Extents base: 1.000000 +Extents range: 4.000000 +Leaves: 8192 +sizeof(btDbvtVolume): 32 bytes +sizeof(btDbvtNode): 44 bytes [1] btDbvtVolume intersections: 3499 ms (-1%) [2] btDbvtVolume merges: 1934 ms (0%) [3] btDbvt::collideTT: 5485 ms (-21%) [4] btDbvt::collideTT self: 2814 ms (-20%) [5] btDbvt::collideTT xform: 7379 ms (-1%) [6] btDbvt::collideTT xform,self: 7270 ms (-2%) -[7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s) +[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s) [8] insert/remove: 2093 ms (0%),(1001983 ir/s) [9] updates (teleport): 1879 ms (-3%),(1116100 u/s) [10] updates (jitter): 1244 ms (-4%),(1685813 u/s) @@ -684,606 +690,606 @@ Benchmarking dbvt... struct btDbvtBenchmark { -struct NilPolicy : btDbvt::ICollide + 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) + ++m_pcount; + if(m_checksort) { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } } - int m_pcount; - btScalar m_depth; - bool m_checksort; + int m_pcount; + btScalar m_depth; + bool m_checksort; }; -struct P14 : btDbvt::ICollide + struct P14 : btDbvt::ICollide { - struct Node + 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 P15 : btDbvt::ICollide { - struct Node + struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; - void Process(const btDbvtNode* leaf) + 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; - t.setOrigin(RandVector3(cs)); - t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); - return(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); } -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) + dbvt.clear(); + for(int i=0;i<leaves;++i) { - dbvt.insert(RandVolume(cs,eb,es),0); + dbvt.insert(RandVolume(cs,eb,es),0); } } }; void btDbvt::benchmark() { -static const btScalar cfgVolumeCenterScale = 100; -static const btScalar cfgVolumeExentsBase = 1; -static const btScalar cfgVolumeExentsScale = 4; -static const int cfgLeaves = 8192; -static const bool cfgEnable = true; + static const btScalar cfgVolumeCenterScale = 100; + static const btScalar cfgVolumeExentsBase = 1; + static const btScalar cfgVolumeExentsScale = 4; + static const int cfgLeaves = 8192; + static const bool cfgEnable = true; -//[1] btDbvtVolume intersections -bool cfgBenchmark1_Enable = cfgEnable; -static const int cfgBenchmark1_Iterations = 8; -static const int cfgBenchmark1_Reference = 3499; -//[2] btDbvtVolume merges -bool cfgBenchmark2_Enable = cfgEnable; -static const int cfgBenchmark2_Iterations = 4; -static const int cfgBenchmark2_Reference = 1945; -//[3] btDbvt::collideTT -bool cfgBenchmark3_Enable = cfgEnable; -static const int cfgBenchmark3_Iterations = 512; -static const int cfgBenchmark3_Reference = 5485; -//[4] btDbvt::collideTT self -bool cfgBenchmark4_Enable = cfgEnable; -static const int cfgBenchmark4_Iterations = 512; -static const int cfgBenchmark4_Reference = 2814; -//[5] btDbvt::collideTT xform -bool cfgBenchmark5_Enable = cfgEnable; -static const int cfgBenchmark5_Iterations = 512; -static const btScalar cfgBenchmark5_OffsetScale = 2; -static const int cfgBenchmark5_Reference = 7379; -//[6] btDbvt::collideTT xform,self -bool cfgBenchmark6_Enable = cfgEnable; -static const int cfgBenchmark6_Iterations = 512; -static const btScalar cfgBenchmark6_OffsetScale = 2; -static const int cfgBenchmark6_Reference = 7270; -//[7] btDbvt::collideRAY -bool cfgBenchmark7_Enable = cfgEnable; -static const int cfgBenchmark7_Passes = 32; -static const int cfgBenchmark7_Iterations = 65536; -static const int cfgBenchmark7_Reference = 6307; -//[8] insert/remove -bool cfgBenchmark8_Enable = cfgEnable; -static const int cfgBenchmark8_Passes = 32; -static const int cfgBenchmark8_Iterations = 65536; -static const int cfgBenchmark8_Reference = 2105; -//[9] updates (teleport) -bool cfgBenchmark9_Enable = cfgEnable; -static const int cfgBenchmark9_Passes = 32; -static const int cfgBenchmark9_Iterations = 65536; -static const int cfgBenchmark9_Reference = 1879; -//[10] updates (jitter) -bool cfgBenchmark10_Enable = cfgEnable; -static const btScalar cfgBenchmark10_Scale = cfgVolumeCenterScale/10000; -static const int cfgBenchmark10_Passes = 32; -static const int cfgBenchmark10_Iterations = 65536; -static const int cfgBenchmark10_Reference = 1244; -//[11] optimize (incremental) -bool cfgBenchmark11_Enable = cfgEnable; -static const int cfgBenchmark11_Passes = 64; -static const int cfgBenchmark11_Iterations = 65536; -static const int cfgBenchmark11_Reference = 2510; -//[12] btDbvtVolume notequal -bool cfgBenchmark12_Enable = cfgEnable; -static const int cfgBenchmark12_Iterations = 32; -static const int cfgBenchmark12_Reference = 3677; -//[13] culling(OCL+fullsort) -bool cfgBenchmark13_Enable = cfgEnable; -static const int cfgBenchmark13_Iterations = 1024; -static const int cfgBenchmark13_Reference = 2231; -//[14] culling(OCL+qsort) -bool cfgBenchmark14_Enable = cfgEnable; -static const int cfgBenchmark14_Iterations = 8192; -static const int cfgBenchmark14_Reference = 3500; -//[15] culling(KDOP+qsort) -bool cfgBenchmark15_Enable = cfgEnable; -static const int cfgBenchmark15_Iterations = 8192; -static const int cfgBenchmark15_Reference = 1151; -//[16] insert/remove batch -bool cfgBenchmark16_Enable = cfgEnable; -static const int cfgBenchmark16_BatchCount = 256; -static const int cfgBenchmark16_Passes = 16384; -static const int cfgBenchmark16_Reference = 5138; -//[17] select -bool cfgBenchmark17_Enable = cfgEnable; -static const int cfgBenchmark17_Iterations = 4; -static const int cfgBenchmark17_Reference = 3390; + //[1] btDbvtVolume intersections + bool cfgBenchmark1_Enable = cfgEnable; + static const int cfgBenchmark1_Iterations = 8; + static const int cfgBenchmark1_Reference = 3499; + //[2] btDbvtVolume merges + bool cfgBenchmark2_Enable = cfgEnable; + static const int cfgBenchmark2_Iterations = 4; + static const int cfgBenchmark2_Reference = 1945; + //[3] btDbvt::collideTT + bool cfgBenchmark3_Enable = cfgEnable; + static const int cfgBenchmark3_Iterations = 512; + static const int cfgBenchmark3_Reference = 5485; + //[4] btDbvt::collideTT self + bool cfgBenchmark4_Enable = cfgEnable; + static const int cfgBenchmark4_Iterations = 512; + static const int cfgBenchmark4_Reference = 2814; + //[5] btDbvt::collideTT xform + bool cfgBenchmark5_Enable = cfgEnable; + static const int cfgBenchmark5_Iterations = 512; + static const btScalar cfgBenchmark5_OffsetScale = 2; + static const int cfgBenchmark5_Reference = 7379; + //[6] btDbvt::collideTT xform,self + bool cfgBenchmark6_Enable = cfgEnable; + static const int cfgBenchmark6_Iterations = 512; + static const btScalar cfgBenchmark6_OffsetScale = 2; + static const int cfgBenchmark6_Reference = 7270; + //[7] btDbvt::rayTest + bool cfgBenchmark7_Enable = cfgEnable; + static const int cfgBenchmark7_Passes = 32; + static const int cfgBenchmark7_Iterations = 65536; + static const int cfgBenchmark7_Reference = 6307; + //[8] insert/remove + bool cfgBenchmark8_Enable = cfgEnable; + static const int cfgBenchmark8_Passes = 32; + static const int cfgBenchmark8_Iterations = 65536; + static const int cfgBenchmark8_Reference = 2105; + //[9] updates (teleport) + bool cfgBenchmark9_Enable = cfgEnable; + static const int cfgBenchmark9_Passes = 32; + static const int cfgBenchmark9_Iterations = 65536; + static const int cfgBenchmark9_Reference = 1879; + //[10] updates (jitter) + bool cfgBenchmark10_Enable = cfgEnable; + static const btScalar cfgBenchmark10_Scale = cfgVolumeCenterScale/10000; + static const int cfgBenchmark10_Passes = 32; + static const int cfgBenchmark10_Iterations = 65536; + static const int cfgBenchmark10_Reference = 1244; + //[11] optimize (incremental) + bool cfgBenchmark11_Enable = cfgEnable; + static const int cfgBenchmark11_Passes = 64; + static const int cfgBenchmark11_Iterations = 65536; + static const int cfgBenchmark11_Reference = 2510; + //[12] btDbvtVolume notequal + bool cfgBenchmark12_Enable = cfgEnable; + static const int cfgBenchmark12_Iterations = 32; + static const int cfgBenchmark12_Reference = 3677; + //[13] culling(OCL+fullsort) + bool cfgBenchmark13_Enable = cfgEnable; + static const int cfgBenchmark13_Iterations = 1024; + static const int cfgBenchmark13_Reference = 2231; + //[14] culling(OCL+qsort) + bool cfgBenchmark14_Enable = cfgEnable; + static const int cfgBenchmark14_Iterations = 8192; + static const int cfgBenchmark14_Reference = 3500; + //[15] culling(KDOP+qsort) + bool cfgBenchmark15_Enable = cfgEnable; + static const int cfgBenchmark15_Iterations = 8192; + static const int cfgBenchmark15_Reference = 1151; + //[16] insert/remove batch + bool cfgBenchmark16_Enable = cfgEnable; + static const int cfgBenchmark16_BatchCount = 256; + static const int cfgBenchmark16_Passes = 16384; + static const int cfgBenchmark16_Reference = 5138; + //[17] select + bool cfgBenchmark17_Enable = cfgEnable; + static const int cfgBenchmark17_Iterations = 4; + static const int cfgBenchmark17_Reference = 3390; -btClock wallclock; -printf("Benchmarking dbvt...\r\n"); -printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale); -printf("\tExtents base: %f\r\n",cfgVolumeExentsBase); -printf("\tExtents range: %f\r\n",cfgVolumeExentsScale); -printf("\tLeaves: %u\r\n",cfgLeaves); -printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume)); -printf("\tsizeof(btDbvtNode): %u bytes\r\n",sizeof(btDbvtNode)); -if(cfgBenchmark1_Enable) + btClock wallclock; + printf("Benchmarking dbvt...\r\n"); + printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale); + printf("\tExtents base: %f\r\n",cfgVolumeExentsBase); + printf("\tExtents range: %f\r\n",cfgVolumeExentsScale); + printf("\tLeaves: %u\r\n",cfgLeaves); + printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume)); + printf("\tsizeof(btDbvtNode): %u bytes\r\n",sizeof(btDbvtNode)); + if(cfgBenchmark1_Enable) {// Benchmark 1 - srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<bool> results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<bool> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) { - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); } - printf("[1] btDbvtVolume intersections: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark1_Iterations;++i) + printf("[1] btDbvtVolume intersections: "); + wallclock.reset(); + 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) + if(cfgBenchmark2_Enable) {// Benchmark 2 - srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<btDbvtVolume> results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<btDbvtVolume> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) { - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); } - printf("[2] btDbvtVolume merges: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark2_Iterations;++i) + printf("[2] btDbvtVolume merges: "); + wallclock.reset(); + 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) + if(cfgBenchmark3_Enable) {// Benchmark 3 - srand(380843); - btDbvt dbvt[2]; - btDbvtBenchmark::NilPolicy policy; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); - dbvt[0].optimizeTopDown(); - dbvt[1].optimizeTopDown(); - printf("[3] btDbvt::collideTT: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark3_Iterations;++i) + srand(380843); + btDbvt dbvt[2]; + btDbvtBenchmark::NilPolicy policy; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[3] btDbvt::collideTT: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark3_Iterations;++i) { - btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy); + 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) + if(cfgBenchmark4_Enable) {// Benchmark 4 - srand(380843); - btDbvt dbvt; - btDbvtBenchmark::NilPolicy policy; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[4] btDbvt::collideTT self: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark4_Iterations;++i) + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::NilPolicy policy; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[4] btDbvt::collideTT self: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark4_Iterations;++i) { - btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy); + 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) + if(cfgBenchmark5_Enable) {// Benchmark 5 - srand(380843); - btDbvt dbvt[2]; - btAlignedObjectArray<btTransform> transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark5_Iterations); - for(int i=0;i<transforms.size();++i) + srand(380843); + btDbvt dbvt[2]; + btAlignedObjectArray<btTransform> transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i<transforms.size();++i) { - transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale); + transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); - dbvt[0].optimizeTopDown(); - dbvt[1].optimizeTopDown(); - printf("[5] btDbvt::collideTT xform: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark5_Iterations;++i) + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[5] btDbvt::collideTT xform: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark5_Iterations;++i) { - btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy); + 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) + if(cfgBenchmark6_Enable) {// Benchmark 6 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btTransform> transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark6_Iterations); - for(int i=0;i<transforms.size();++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btTransform> transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i<transforms.size();++i) { - transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale); + transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[6] btDbvt::collideTT xform,self: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark6_Iterations;++i) + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[6] btDbvt::collideTT xform,self: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark6_Iterations;++i) { - btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy); + 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) + if(cfgBenchmark7_Enable) {// Benchmark 7 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> rayorg; - btAlignedObjectArray<btVector3> raydir; - btDbvtBenchmark::NilPolicy policy; - rayorg.resize(cfgBenchmark7_Iterations); - raydir.resize(cfgBenchmark7_Iterations); - for(int i=0;i<rayorg.size();++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> rayorg; + btAlignedObjectArray<btVector3> raydir; + btDbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i<rayorg.size();++i) { - rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); - raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); + rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); + raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[7] btDbvt::collideRAY: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark7_Passes;++i) + 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 j=0;j<cfgBenchmark7_Iterations;++j) + for(int j=0;j<cfgBenchmark7_Iterations;++j) { - btDbvt::collideRAY(dbvt.m_root,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) + if(cfgBenchmark8_Enable) {// Benchmark 8 - srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[8] insert/remove: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark8_Passes;++i) + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[8] insert/remove: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark8_Passes;++i) { - for(int j=0;j<cfgBenchmark8_Iterations;++j) + 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) + if(cfgBenchmark9_Enable) {// Benchmark 9 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<const btDbvtNode*> leaves; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); - printf("[9] updates (teleport): "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark9_Passes;++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<const btDbvtNode*> leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[9] updates (teleport): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark9_Passes;++i) { - for(int j=0;j<cfgBenchmark9_Iterations;++j) + 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) + if(cfgBenchmark10_Enable) {// Benchmark 10 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<const btDbvtNode*> leaves; - btAlignedObjectArray<btVector3> vectors; - vectors.resize(cfgBenchmark10_Iterations); - for(int i=0;i<vectors.size();++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<const btDbvtNode*> leaves; + btAlignedObjectArray<btVector3> vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i<vectors.size();++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale; + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale; } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); - printf("[10] updates (jitter): "); - wallclock.reset(); - - for(int i=0;i<cfgBenchmark10_Passes;++i) + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[10] updates (jitter): "); + wallclock.reset(); + + for(int i=0;i<cfgBenchmark10_Passes;++i) { - for(int j=0;j<cfgBenchmark10_Iterations;++j) + 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 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) + if(cfgBenchmark11_Enable) {// Benchmark 11 - srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[11] optimize (incremental): "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark11_Passes;++i) + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[11] optimize (incremental): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark11_Passes;++i) { - dbvt.optimizeIncremental(cfgBenchmark11_Iterations); + 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) + if(cfgBenchmark12_Enable) {// Benchmark 12 - srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<bool> results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<bool> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) { - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); } - printf("[12] btDbvtVolume notequal: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark12_Iterations;++i) + printf("[12] btDbvtVolume notequal: "); + wallclock.reset(); + 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) + if(cfgBenchmark13_Enable) {// Benchmark 13 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> vectors; - btDbvtBenchmark::NilPolicy policy; - vectors.resize(cfgBenchmark13_Iterations); - for(int i=0;i<vectors.size();++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i<vectors.size();++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[13] culling(OCL+fullsort): "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark13_Iterations;++i) + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[13] culling(OCL+fullsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark13_Iterations;++i) { - static const btScalar offset=0; - policy.m_depth=-SIMD_INFINITY; - dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy); + 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) + if(cfgBenchmark14_Enable) {// Benchmark 14 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> vectors; - btDbvtBenchmark::P14 policy; - vectors.resize(cfgBenchmark14_Iterations); - for(int i=0;i<vectors.size();++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i<vectors.size();++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - policy.m_nodes.reserve(cfgLeaves); - printf("[14] culling(OCL+qsort): "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark14_Iterations;++i) + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[14] culling(OCL+qsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark14_Iterations;++i) { - static const btScalar offset=0; - policy.m_nodes.resize(0); - dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false); - policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc); + static const btScalar offset=0; + policy.m_nodes.resize(0); + dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false); + policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc); } - const int time=(int)wallclock.getTimeMilliseconds(); - const int t=cfgBenchmark14_Iterations; - printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time); + 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) + if(cfgBenchmark15_Enable) {// Benchmark 15 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btVector3> vectors; - btDbvtBenchmark::P15 policy; - vectors.resize(cfgBenchmark15_Iterations); - for(int i=0;i<vectors.size();++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btVector3> vectors; + btDbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i<vectors.size();++i) { - vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); + vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized(); } - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - policy.m_nodes.reserve(cfgLeaves); - printf("[15] culling(KDOP+qsort): "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark15_Iterations;++i) + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[15] culling(KDOP+qsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark15_Iterations;++i) { - static const btScalar offset=0; - policy.m_nodes.resize(0); - policy.m_axis=vectors[i]; - dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy); - policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc); + static const btScalar offset=0; + policy.m_nodes.resize(0); + policy.m_axis=vectors[i]; + dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy); + policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc); } - const int time=(int)wallclock.getTimeMilliseconds(); - const int t=cfgBenchmark15_Iterations; - printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time); + 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) + if(cfgBenchmark16_Enable) {// Benchmark 16 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray<btDbvtNode*> batch; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - batch.reserve(cfgBenchmark16_BatchCount); - printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); - wallclock.reset(); - for(int i=0;i<cfgBenchmark16_Passes;++i) + srand(380843); + btDbvt dbvt; + btAlignedObjectArray<btDbvtNode*> batch; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + batch.reserve(cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + wallclock.reset(); + for(int i=0;i<cfgBenchmark16_Passes;++i) { - for(int j=0;j<cfgBenchmark16_BatchCount;++j) + 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]); + dbvt.remove(batch[j]); } - batch.resize(0); + 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) + if(cfgBenchmark17_Enable) {// Benchmark 17 - srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<int> results; - btAlignedObjectArray<int> indices; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - indices.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) + srand(380843); + btAlignedObjectArray<btDbvtVolume> volumes; + btAlignedObjectArray<int> results; + btAlignedObjectArray<int> indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) { - indices[i]=i; - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + 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) + printf("[17] btDbvtVolume select: "); + wallclock.reset(); + 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"); + printf("\r\n\r\n"); } #endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h index 21d69acf151..d3cf1e75039 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -20,6 +20,7 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" +#include "LinearMath/btAabbUtil2.h" // // Compile time configuration @@ -31,11 +32,11 @@ subject to the following restrictions: #define DBVT_IMPL_SSE 1 // SSE // Template implementation of ICollide -#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds - #if (defined (_MSC_VER) && _MSC_VER >= 1400) - #define DBVT_USE_TEMPLATE 1 - #else - #define DBVT_USE_TEMPLATE 0 +#ifdef WIN32 +#if (defined (_MSC_VER) && _MSC_VER >= 1400) +#define DBVT_USE_TEMPLATE 1 +#else +#define DBVT_USE_TEMPLATE 0 #endif #else #define DBVT_USE_TEMPLATE 0 @@ -52,16 +53,11 @@ subject to the following restrictions: // Inlining #define DBVT_INLINE SIMD_FORCE_INLINE -// Align -#ifdef WIN32 -#define DBVT_ALIGN __declspec(align(16)) -#else -#define DBVT_ALIGN -#endif // Specific methods implementation -#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds +//SSE gives errors on a MSVC 7.1 +#ifdef BT_USE_SSE #define DBVT_SELECT_IMPL DBVT_IMPL_SSE #define DBVT_MERGE_IMPL DBVT_IMPL_SSE #define DBVT_INT0_IMPL DBVT_IMPL_SSE @@ -86,7 +82,7 @@ subject to the following restrictions: #define DBVT_VIRTUAL_DTOR(a) #define DBVT_PREFIX template <typename T> #define DBVT_IPOLICY T& policy -#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0; +#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; #else #define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} #define DBVT_VIRTUAL virtual @@ -133,46 +129,41 @@ subject to the following restrictions: /* btDbvtAabbMm */ struct btDbvtAabbMm { -DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } -DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } -DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } -DBVT_INLINE const btVector3& Mins() const { return(mi); } -DBVT_INLINE const btVector3& Maxs() const { return(mx); } -static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); -static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); -static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); -static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); -static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); -DBVT_INLINE void Expand(const btVector3& e); -DBVT_INLINE void SignedExpand(const btVector3& e); -DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; -DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; -DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; -DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); -DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - const btTransform& xform); -DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btVector3& b); -DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btVector3& org, - const btVector3& invdir, - const unsigned* signs); -DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); -DBVT_INLINE friend int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b); -DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r); -DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); + 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); 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 @@ -185,88 +176,94 @@ struct btDbvtNode 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; - }; + union + { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; + }; }; ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. struct btDbvt - { +{ /* Stack element */ struct sStkNN - { + { const btDbvtNode* a; const btDbvtNode* b; sStkNN() {} sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} - }; + }; struct sStkNP - { + { const btDbvtNode* node; int mask; sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} - }; + }; struct sStkNPS - { + { const btDbvtNode* node; int mask; btScalar value; sStkNPS() {} sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} - }; + }; struct sStkCLN - { + { const btDbvtNode* node; btDbvtNode* parent; sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} - }; + }; // Policies/Interfaces - + /* ICollide */ struct ICollide - { + { DBVT_VIRTUAL_DTOR(ICollide) - DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } - }; + }; /* IWriter */ struct IWriter - { + { virtual ~IWriter() {} virtual void Prepare(const btDbvtNode* root,int numnodes)=0; virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; - }; + }; /* IClone */ struct IClone - { + { virtual ~IClone() {} virtual void CloneLeaf(btDbvtNode*) {} - }; - + }; + // Constants enum { - SIMPLE_STACKSIZE = 64, - DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 - }; - + 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; + + // Methods - btDbvt(); - ~btDbvt(); + btDbvt(); + ~btDbvt(); void clear(); bool empty() const { return(0==m_root); } void optimizeBottomUp(); @@ -274,95 +271,118 @@ struct btDbvt void optimizeIncremental(int passes); btDbvtNode* insert(const btDbvtVolume& box,void* data); void update(btDbvtNode* leaf,int lookahead=-1); - void update(btDbvtNode* leaf,const btDbvtVolume& volume); - bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin); - bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity); - bool update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin); + void 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 +#if DBVT_ENABLE_BENCHMARK static void benchmark(); - #else +#else static void benchmark(){} - #endif +#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); DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); + void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); + DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); +#if 0 DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY); + void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY); DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY); + void collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY); +#endif + DBVT_PREFIX - static void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - DBVT_IPOLICY); + void collideTV( const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY); + ///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 collideRAY( const btDbvtNode* root, - const btVector3& origin, - const btVector3& direction, - 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 - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); + 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; + 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 collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); DBVT_PREFIX - static void collideTU( const btDbvtNode* root, - DBVT_IPOLICY); + static void collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort=true); + DBVT_PREFIX + static void collideTU( const btDbvtNode* root, + DBVT_IPOLICY); // Helpers static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) - { + { int m=0; while(l<h) - { + { m=(l+h)>>1; if(a[i[m]].value>=v) l=m+1; else h=m; - } - return(h); } + return(h); + } static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree, - btAlignedObjectArray<sStkNPS>& stock, - const sStkNPS& value) - { + btAlignedObjectArray<sStkNPS>& stock, + const sStkNPS& value) + { int i; if(ifree.size()>0) - { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } - else - { i=stock.size();stock.push_back(value); } + { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + else + { i=stock.size();stock.push_back(value); } return(i); - } + } // - private: - btDbvt(const btDbvt&) {} - }; +private: + btDbvt(const btDbvt&) {} +}; // // Inline's @@ -371,69 +391,69 @@ struct btDbvt // inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) { -btDbvtAabbMm box; -box.mi=c-e;box.mx=c+e; -return(box); + btDbvtAabbMm box; + box.mi=c-e;box.mx=c+e; + return(box); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) { -return(FromCE(c,btVector3(r,r,r))); + return(FromCE(c,btVector3(r,r,r))); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) { -btDbvtAabbMm box; -box.mi=mi;box.mx=mx; -return(box); + btDbvtAabbMm box; + box.mi=mi;box.mx=mx; + return(box); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) { -btDbvtAabbMm box; -box.mi=box.mx=pts[0]; -for(int i=1;i<n;++i) + btDbvtAabbMm box; + box.mi=box.mx=pts[0]; + for(int i=1;i<n;++i) { - box.mi.setMin(pts[i]); - box.mx.setMax(pts[i]); + box.mi.setMin(pts[i]); + box.mx.setMax(pts[i]); } -return(box); + return(box); } // inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n) { -btDbvtAabbMm box; -box.mi=box.mx=*ppts[0]; -for(int i=1;i<n;++i) + btDbvtAabbMm box; + box.mi=box.mx=*ppts[0]; + for(int i=1;i<n;++i) { - box.mi.setMin(*ppts[i]); - box.mx.setMax(*ppts[i]); + box.mi.setMin(*ppts[i]); + box.mx.setMax(*ppts[i]); } -return(box); + return(box); } // DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) { -mi-=e;mx+=e; + mi-=e;mx+=e; } - + // DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e) { -if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); -if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); -if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); + if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); + if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); + if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); } - + // DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const { -return( (mi.x()<=a.mi.x())&& + return( (mi.x()<=a.mi.x())&& (mi.y()<=a.mi.y())&& (mi.z()<=a.mi.z())&& (mx.x()>=a.mx.x())&& @@ -444,64 +464,64 @@ return( (mi.x()<=a.mi.x())&& // 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + pi=btVector3(mi.x(),mi.y(),mi.z());break; } -if((dot(n,px)+o)<0) return(-1); -if((dot(n,pi)+o)>=0) return(+1); -return(0); + if((dot(n,px)+o)<0) return(-1); + if((dot(n,pi)+o)>=0) return(+1); + return(0); } // DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const { -const btVector3* b[]={&mx,&mi}; -const btVector3 p( b[(signs>>0)&1]->x(), - b[(signs>>1)&1]->y(), - b[(signs>>2)&1]->z()); -return(dot(p,v)); + const btVector3* b[]={&mx,&mi}; + const btVector3 p( b[(signs>>0)&1]->x(), + b[(signs>>1)&1]->y(), + b[(signs>>2)&1]->z()); + return(dot(p,v)); } // DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const { -for(int i=0;i<3;++i) + for(int i=0;i<3;++i) { - if(d[i]<0) + if(d[i]<0) { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } else { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } } } - + // DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& b) { #if DBVT_INT0_IMPL == DBVT_IMPL_SSE -const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), - _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); -const __int32* pu((const __int32*)&rt); -return((pu[0]|pu[1]|pu[2])==0); + const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); + const __int32* pu((const __int32*)&rt); + return((pu[0]|pu[1]|pu[2])==0); #else -return( (a.mi.x()<=b.mx.x())&& + 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())&& @@ -510,27 +530,13 @@ return( (a.mi.x()<=b.mx.x())&& #endif } -// -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - const btTransform& xform) -{ -const btVector3 d0=xform*b.Center()-a.Center(); -const btVector3 d1=d0*xform.getBasis(); -btScalar s0[2]={0,0}; -btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]}; -a.AddSpan(d0,s0[0],s0[1]); -b.AddSpan(d1,s1[0],s1[1]); -if(s0[0]>(s1[1])) return(false); -if(s0[1]<(s1[0])) return(false); -return(true); -} + // DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& b) + const btVector3& b) { -return( (b.x()>=a.mi.x())&& + return( (b.x()>=a.mi.x())&& (b.y()>=a.mi.y())&& (b.z()>=a.mi.z())&& (b.x()<=a.mx.x())&& @@ -538,55 +544,40 @@ return( (b.x()>=a.mi.x())&& (b.z()<=a.mx.z())); } -// -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& org, - const btVector3& invdir, - const unsigned* signs) -{ -#if 0 -const btVector3 b0((a.mi-org)*invdir); -const btVector3 b1((a.mx-org)*invdir); -const btVector3 tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2])); -const btVector3 tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2])); -const btScalar tin=btMax(tmin[0],btMax(tmin[1],tmin[2])); -const btScalar tout=btMin(tmax[0],btMin(tmax[1],tmax[2])); -return(tin<tout); -#else -const btVector3* bounds[2]={&a.mi,&a.mx}; -btScalar txmin=(bounds[ signs[0]]->x()-org[0])*invdir[0]; -btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0]; -const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1]; -const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1]; -if((txmin>tymax)||(tymin>txmax)) return(false); -if(tymin>txmin) txmin=tymin; -if(tymax<txmax) txmax=tymax; -const btScalar tzmin=(bounds[ signs[2]]->z()-org[2])*invdir[2]; -const btScalar tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2]; -if((txmin>tzmax)||(tzmin>txmax)) return(false); -if(tzmin>txmin) txmin=tzmin; -if(tzmax<txmax) txmax=tzmax; -return(txmax>0); -#endif -} - + + + + +////////////////////////////////////// + + // DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const 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) + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE -static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; - // TODO: the intrinsic version is 11% slower - #if DBVT_USE_INTRINSIC_SSE + static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; + ///@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 + { + __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)); @@ -600,74 +591,78 @@ static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7ffffff __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)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); bmi=_mm_add_ps(bmi,t1); bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); - return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); - #else - DBVT_ALIGN __int32 r[1]; + + btSSEUnion tmp; + tmp.ssereg = _mm_cmple_ss(bmi,ami); + return tmp.ints[0]&1; + +#else + ATTRIBUTE_ALIGNED16(__int32 r[1]); __asm - { + { mov eax,o - mov ecx,a - mov edx,b - movaps xmm0,[eax] + mov ecx,a + mov edx,b + movaps xmm0,[eax] movaps xmm5,mask - addps xmm0,[eax+16] + addps xmm0,[eax+16] movaps xmm1,[ecx] movaps xmm2,[edx] addps xmm1,[ecx+16] addps xmm2,[edx+16] subps xmm1,xmm0 - subps xmm2,xmm0 - andps xmm1,xmm5 - andps xmm2,xmm5 - movhlps xmm3,xmm1 - movhlps xmm4,xmm2 - addps xmm1,xmm3 - addps xmm2,xmm4 - pshufd xmm3,xmm1,1 - pshufd xmm4,xmm2,1 - addss xmm1,xmm3 - addss xmm2,xmm4 - cmpless xmm2,xmm1 - movss r,xmm2 - } + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } return(r[0]&1); - #endif +#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) + 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); + __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) + const btDbvtAabbMm& b) { -return( (a.mi.x()!=b.mi.x())|| + 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())|| @@ -682,235 +677,383 @@ return( (a.mi.x()!=b.mi.x())|| // DBVT_PREFIX inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -policy.Process(root); -if(root->isinternal()) + DBVT_CHECKTYPE + 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) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root->isinternal()) - { - enumLeaves(root->childs[0],policy); - enumLeaves(root->childs[1],policy); - } - else - { - policy.Process(root); - } + DBVT_CHECKTYPE + if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } } // DBVT_PREFIX inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) + const btDbvtNode* root1, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root0&&root1) - { - btAlignedObjectArray<sStkNN> stack; - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - stack.resize(DOUBLE_STACKSIZE); - stack[0]=sStkNN(root0,root1); - do { - sStkNN p=stack[--depth]; - if(depth>treshold) - { - stack.resize(stack.size()*2); - treshold=stack.size()-4; - } - if(p.a==p.b) - { - if(p.a->isinternal()) + DBVT_CHECKTYPE + if(root0&&root1) + { + 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) { - stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; } - } - else if(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) + if(p.a==p.b) { - if(p.b->isinternal()) + if(p.a->isinternal()) { - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stack[depth++]=sStkNN(p.a->childs[0],p.b); - stack[depth++]=sStkNN(p.a->childs[1],p.b); + 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 + else if(Intersect(p.a->volume,p.b->volume)) { - if(p.b->isinternal()) + if(p.a->isinternal()) { - stack[depth++]=sStkNN(p.a,p.b->childs[0]); - stack[depth++]=sStkNN(p.a,p.b->childs[1]); + 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 { - policy.Process(p.a,p.b); + 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); - } + } while(depth); + } } -// + + DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY) +inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root0&&root1) - { - btAlignedObjectArray<sStkNN> stack; - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - stack.resize(DOUBLE_STACKSIZE); - stack[0]=sStkNN(root0,root1); - do { - sStkNN p=stack[--depth]; - if(Intersect(p.a->volume,p.b->volume,xform)) - { - if(depth>treshold) + DBVT_CHECKTYPE + if(root0&&root1) + { + 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) { - stack.resize(stack.size()*2); - treshold=stack.size()-4; + m_stkStack.resize(m_stkStack.size()*2); + treshold=m_stkStack.size()-4; } - if(p.a->isinternal()) + if(p.a==p.b) { - if(p.b->isinternal()) - { - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + if(p.a->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]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->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); + } } else { - stack[depth++]=sStkNN(p.a->childs[0],p.b); - stack[depth++]=sStkNN(p.a->childs[1],p.b); + 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); + } } } - else + } while(depth); + } +} + +#if 0 +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + 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(Intersect(p.a->volume,p.b->volume,xform)) { - if(p.b->isinternal()) + if(depth>treshold) { - stack[depth++]=sStkNN(p.a,p.b->childs[0]); - stack[depth++]=sStkNN(p.a,p.b->childs[1]); + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + 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 { - policy.Process(p.a,p.b); + 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); - } + } while(depth); + } } - // DBVT_PREFIX inline void btDbvt::collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY) + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY) { -const btTransform xform=xform0.inverse()*xform1; -collideTT(root0,root1,xform,policy); + const btTransform xform=xform0.inverse()*xform1; + collideTT(root0,root1,xform,policy); } +#endif // DBVT_PREFIX inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) + const btDbvtVolume& vol, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + 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)) + { + 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::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 { -DBVT_CHECKTYPE -if(root) + DBVT_CHECKTYPE + if(root) { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray<const btDbvtNode*> stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->volume,volume)) + btVector3 resultNormal; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + btAlignedObjectArray<const btDbvtNode*> stack; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; + btVector3 bounds[2]; + do + { + const btDbvtNode* node=stack[--depth]; + bounds[0] = node->volume.Mins()+aabbMin; + bounds[1] = node->volume.Maxs()+aabbMax; + 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(n->isinternal()) + if(node->isinternal()) { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; } else { - policy.Process(n); + policy.Process(node); } } - } while(stack.size()>0); + } while(depth); } } // DBVT_PREFIX -inline void btDbvt::collideRAY( const btDbvtNode* root, - const btVector3& origin, - const btVector3& direction, - DBVT_IPOLICY) +inline void btDbvt::rayTest( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - const btVector3 normal=direction.normalized(); - const btVector3 invdir( 1/normal.x(), - 1/normal.y(), - 1/normal.z()); - const unsigned signs[]={ direction.x()<0, - direction.y()<0, - direction.z()<0}; - btAlignedObjectArray<const btDbvtNode*> stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* node=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(node->volume,origin,invdir,signs)) - { - if(node->isinternal()) - { - stack.push_back(node->childs[0]); - stack.push_back(node->childs[1]); - } - else + DBVT_CHECKTYPE + if(root) + { + btVector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : 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); + + btVector3 resultNormal; + + btAlignedObjectArray<const btDbvtNode*> stack; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + + stack.resize(DOUBLE_STACKSIZE); + 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); + +#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 + + if(result1) { - policy.Process(node); + 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 + { + policy.Process(node); + } } - } - } while(stack.size()); - } + } while(depth); + + } } // @@ -921,174 +1064,174 @@ inline void btDbvt::collideKDOP(const btDbvtNode* root, int count, DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - const int inside=(1<<count)-1; - btAlignedObjectArray<sStkNP> stack; - int signs[sizeof(unsigned)*8]; - btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); - for(int i=0;i<count;++i) + DBVT_CHECKTYPE + if(root) { - signs[i]= ((normals[i].x()>=0)?1:0)+ + const int inside=(1<<count)-1; + btAlignedObjectArray<sStkNP> stack; + int signs[sizeof(unsigned)*8]; + btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i<count;++i) + { + signs[i]= ((normals[i].x()>=0)?1:0)+ ((normals[i].y()>=0)?2:0)+ ((normals[i].z()>=0)?4:0); - } - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(sStkNP(root,0)); - do { - sStkNP se=stack[stack.size()-1]; - bool out=false; - stack.pop_back(); - for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) - { - if(0==(se.mask&j)) + } + 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) { - const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); - switch(side) + if(0==(se.mask&j)) { - case -1: out=true;break; - case +1: se.mask|=j;break; + const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } } } - } - if(!out) - { - if((se.mask!=inside)&&(se.node->isinternal())) - { - stack.push_back(sStkNP(se.node->childs[0],se.mask)); - stack.push_back(sStkNP(se.node->childs[1],se.mask)); - } - else + if(!out) { - if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + if((se.mask!=inside)&&(se.node->isinternal())) + { + stack.push_back(sStkNP(se.node->childs[0],se.mask)); + stack.push_back(sStkNP(se.node->childs[1],se.mask)); + } + else + { + if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + } } - } - } while(stack.size()); - } + } 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) + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) { -DBVT_CHECKTYPE -if(root) - { - const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ - (sortaxis[1]>=0?2:0)+ - (sortaxis[2]>=0?4:0); - const int inside=(1<<count)-1; - btAlignedObjectArray<sStkNPS> stock; - btAlignedObjectArray<int> ifree; - btAlignedObjectArray<int> stack; - int signs[sizeof(unsigned)*8]; - btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); - for(int i=0;i<count;++i) + DBVT_CHECKTYPE + if(root) { - signs[i]= ((normals[i].x()>=0)?1:0)+ + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1<<count)-1; + btAlignedObjectArray<sStkNPS> stock; + btAlignedObjectArray<int> ifree; + btAlignedObjectArray<int> stack; + int signs[sizeof(unsigned)*8]; + btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i<count;++i) + { + signs[i]= ((normals[i].x()>=0)?1:0)+ ((normals[i].y()>=0)?2:0)+ ((normals[i].z()>=0)?4:0); - } - stock.reserve(SIMPLE_STACKSIZE); - stack.reserve(SIMPLE_STACKSIZE); - ifree.reserve(SIMPLE_STACKSIZE); - stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); - do { - const int id=stack[stack.size()-1]; - sStkNPS se=stock[id]; - stack.pop_back();ifree.push_back(id); - if(se.mask!=inside) - { - bool out=false; - for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + } + 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) { - if(0==(se.mask&j)) + bool out=false; + for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) { - const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); - switch(side) + if(0==(se.mask&j)) { - case -1: out=true;break; - case +1: se.mask|=j;break; + const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } } } + if(out) continue; } - if(out) continue; - } - if(policy.Descent(se.node)) - { - if(se.node->isinternal()) + if(policy.Descent(se.node)) { - 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)) + if(se.node->isinternal()) { - /* Insert 0 */ - j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); - stack.push_back(0); - #if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); - #else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; - #endif - stack[j]=allocate(ifree,stock,nes[q]); - /* Insert 1 */ - j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); - stack.push_back(0); - #if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); - #else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; - #endif - stack[j]=allocate(ifree,stock,nes[1-q]); + const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; + sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), + sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; + const int q=nes[0].value<nes[1].value?1:0; + int j=stack.size(); + if(fsort&&(j>0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[q]); + /* Insert 1 */ + j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[1-q]); + } + else + { + stack.push_back(allocate(ifree,stock,nes[q])); + stack.push_back(allocate(ifree,stock,nes[1-q])); + } } else { - stack.push_back(allocate(ifree,stock,nes[q])); - stack.push_back(allocate(ifree,stock,nes[1-q])); + policy.Process(se.node,se.value); } } - else - { - policy.Process(se.node,se.value); - } - } - } while(stack.size()); - } + } while(stack.size()); + } } // DBVT_PREFIX inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - btAlignedObjectArray<const btDbvtNode*> stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(policy.Descent(n)) - { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { policy.Process(n); } - } - } while(stack.size()>0); - } + DBVT_CHECKTYPE + if(root) + { + btAlignedObjectArray<const btDbvtNode*> stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } } // diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index e00fc6aa5e3..f231717af17 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -26,19 +26,19 @@ subject to the following restrictions: #if DBVT_BP_PROFILE struct ProfileScope - { +{ __forceinline ProfileScope(btClock& clock,unsigned long& value) : - m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) - { - } + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } __forceinline ~ProfileScope() - { + { (*m_value)+=m_clock->getTimeMicroseconds()-m_base; - } + } btClock* m_clock; unsigned long* m_value; unsigned long m_base; - }; +}; #define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) #else #define SPC(_value_) @@ -52,35 +52,35 @@ struct ProfileScope template <typename T> static inline void listappend(T* item,T*& list) { -item->links[0]=0; -item->links[1]=list; -if(list) list->links[0]=item; -list=item; + item->links[0]=0; + item->links[1]=list; + if(list) list->links[0]=item; + list=item; } // template <typename T> static inline void listremove(T* item,T*& list) { -if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; -if(item->links[1]) item->links[1]->links[0]=item->links[0]; + if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; + if(item->links[1]) item->links[1]->links[0]=item->links[0]; } // template <typename T> static inline int listcount(T* root) { -int n=0; -while(root) { ++n;root=root->links[1]; } -return(n); + int n=0; + while(root) { ++n;root=root->links[1]; } + return(n); } // template <typename T> static inline void clear(T& value) { -static const struct ZeroDummy : T {} zerodummy; -value=zerodummy; + static const struct ZeroDummy : T {} zerodummy; + value=zerodummy; } // @@ -90,25 +90,26 @@ value=zerodummy; /* Tree collider */ struct btDbvtTreeCollider : btDbvt::ICollide { -btDbvtBroadphase* pbp; -btDbvtProxy* proxy; - btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} -void Process(const btDbvtNode* na,const btDbvtNode* nb) + btDbvtBroadphase* pbp; + btDbvtProxy* proxy; + btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} + void Process(const btDbvtNode* na,const btDbvtNode* nb) { - if(na!=nb) + if(na!=nb) { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; - #if DBVT_BP_SORTPAIRS - if(pa>pb) btSwap(pa,pb); - #endif - pbp->m_paircache->addOverlappingPair(pa,pb); - ++pbp->m_newpairs; + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + 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); } }; @@ -119,147 +120,197 @@ void Process(const btDbvtNode* n) // btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) { -m_deferedcollide = false; -m_needcleanup = true; -m_releasepaircache = (paircache!=0)?false:true; -m_prediction = 1/(btScalar)2; -m_stageCurrent = 0; -m_fixedleft = 0; -m_fupdates = 1; -m_dupdates = 0; -m_cupdates = 10; -m_newpairs = 1; -m_updates_call = 0; -m_updates_done = 0; -m_updates_ratio = 0; -m_paircache = paircache? - paircache : - new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); -m_gid = 0; -m_pid = 0; -m_cid = 0; -for(int i=0;i<=STAGECOUNT;++i) + m_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache!=0)?false:true; + m_prediction = 1/(btScalar)2; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); + m_gid = 0; + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) { - m_stageRoots[i]=0; + m_stageRoots[i]=0; } #if DBVT_BP_PROFILE -clear(m_profiling); + clear(m_profiling); #endif } // btDbvtBroadphase::~btDbvtBroadphase() { -if(m_releasepaircache) -{ - m_paircache->~btOverlappingPairCache(); - btAlignedFree(m_paircache); -} + if(m_releasepaircache) + { + m_paircache->~btOverlappingPairCache(); + btAlignedFree(m_paircache); + } } // btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, - const btVector3& aabbMax, - int /*shapeType*/, - void* userPtr, - short int collisionFilterGroup, - short int collisionFilterMask, - btDispatcher* /*dispatcher*/, - void* /*multiSapProxy*/) + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + short int collisionFilterGroup, + short int collisionFilterMask, + btDispatcher* /*dispatcher*/, + void* /*multiSapProxy*/) { -btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( userPtr, - collisionFilterGroup, - collisionFilterMask); -proxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); -proxy->stage = m_stageCurrent; -proxy->m_uniqueId = ++m_gid; -proxy->leaf = m_sets[0].insert(proxy->aabb,proxy); -listappend(proxy,m_stageRoots[m_stageCurrent]); -if(!m_deferedcollide) + btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, + collisionFilterGroup, + collisionFilterMask); + + 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) { - btDbvtTreeCollider collider(this); - collider.proxy=proxy; - btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider); + 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) + btDispatcher* dispatcher) { -btDbvtProxy* proxy=(btDbvtProxy*)absproxy; -if(proxy->stage==STAGECOUNT) - m_sets[1].remove(proxy->leaf); + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); else - m_sets[0].remove(proxy->leaf); -listremove(proxy,m_stageRoots[proxy->stage]); -m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); -btAlignedFree(proxy); -m_needcleanup=true; + m_sets[0].remove(proxy->leaf); + listremove(proxy,m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); + btAlignedFree(proxy); + m_needcleanup=true; +} + +void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} + +struct BroadphaseRayTester : btDbvt::ICollide +{ + btBroadphaseRayCallback& m_rayCallback; + BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) + :m_rayCallback(orgCallback) + { + } + void Process(const btDbvtNode* leaf) + { + 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) +{ + BroadphaseRayTester callback(rayCallback); + + 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); + } // void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) + 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) + 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; + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; } else {/* dynamic set */ - ++m_updates_call; - if(Intersect(proxy->leaf->volume,aabb)) + ++m_updates_call; + if(Intersect(proxy->leaf->volume,aabb)) {/* Moving */ - const btVector3 delta=aabbMin-proxy->aabb.Mins(); - btVector3 velocity(aabb.Extents()*m_prediction); - if(delta[0]<0) velocity[0]=-velocity[0]; - if(delta[1]<0) velocity[1]=-velocity[1]; - if(delta[2]<0) velocity[2]=-velocity[2]; - if ( - #ifdef DBVT_BP_MARGIN - m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) - #else - m_sets[0].update(proxy->leaf,aabb,velocity) - #endif - ) + + 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 + ) { - ++m_updates_done; - docollide=true; + ++m_updates_done; + docollide=true; } } else {/* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); - ++m_updates_done; - docollide=true; + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; } } - listremove(proxy,m_stageRoots[proxy->stage]); - proxy->aabb = aabb; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) + 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) { - m_needcleanup=true; - if(!m_deferedcollide) + m_needcleanup=true; + if(!m_deferedcollide) { - btDbvtTreeCollider collider(this); - btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider); - btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider); + 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); } } } @@ -268,132 +319,226 @@ if(NotEqual(aabb,proxy->leaf->volume)) // void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { -collide(dispatcher); + collide(dispatcher); #if DBVT_BP_PROFILE -if(0==(m_pid%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(); + 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(); + + //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++) + { + + btBroadphasePair& pair = overlappingPairArray[i]; + + bool isDuplicate = (pair == previousPair); + + previousPair = pair; + + bool needsRemoval = false; + + 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); + + if (hasOverlap) + { + needsRemoval = false; + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + m_paircache->cleanOverlappingPair(pair,dispatcher); + + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + invalidPair++; + } + + } + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); + } } // 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) + /*printf("---------------------------------------------------------\n"); + printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); + printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves); + printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs()); { - 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); + int i; + for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++) + { + printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), + getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); + } + printf("\n"); } -/* dynamic -> fixed set */ -m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; -btDbvtProxy* current=m_stageRoots[m_stageCurrent]; -if(current) +*/ + + + + SPC(m_profiling.m_total); + /* optimize */ + m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); + if(m_fixedleft) + { + const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; + m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); + m_fixedleft=btMax<int>(0,m_fixedleft-count); + } + /* dynamic -> fixed set */ + m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; + btDbvtProxy* current=m_stageRoots[m_stageCurrent]; + if(current) { - btDbvtTreeCollider collider(this); - do { - btDbvtProxy* next=current->links[1]; - listremove(current,m_stageRoots[current->stage]); - listappend(current,m_stageRoots[STAGECOUNT]); - #if DBVT_BP_ACCURATESLEEPING - m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); - collider.proxy=current; - btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); - #endif - m_sets[0].remove(current->leaf); - current->leaf = m_sets[1].insert(current->aabb,current); - current->stage = STAGECOUNT; - current = next; + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); +#if DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); +#endif + m_sets[0].remove(current->leaf); + 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; + 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); - btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); + SPC(m_profiling.m_fdcollide); + 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); - btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); + SPC(m_profiling.m_ddcollide); + 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) + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) { - const int ci=pairs.size(); - int ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100)); - for(int i=0;i<ni;++i) + + 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)%ci]; - 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>pb) btSwap(pa,pb); - #endif - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + m_paircache->removeOverlappingPair(pa,pb,dispatcher); + --ni;--i; } } - if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + 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_pid; + m_newpairs=1; + m_needcleanup=false; + if(m_updates_call>0) { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } else { m_updates_ratio=0; } -m_updates_done/=2; -m_updates_call/=2; + m_updates_done/=2; + m_updates_call/=2; } // void btDbvtBroadphase::optimize() { -m_sets[0].optimizeTopDown(); -m_sets[1].optimizeTopDown(); + m_sets[0].optimizeTopDown(); + m_sets[1].optimizeTopDown(); } // btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() { -return(m_paircache); + return(m_paircache); } // const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const { -return(m_paircache); + return(m_paircache); } // @@ -402,16 +547,49 @@ void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aab ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; -if(!m_sets[0].empty()) - if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, - m_sets[1].m_root->volume,bounds); - else - bounds=m_sets[0].m_root->volume; -else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; - else - bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); -aabbMin=bounds.Mins(); -aabbMax=bounds.Maxs(); + if(!m_sets[0].empty()) + if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, + m_sets[1].m_root->volume,bounds); + else + bounds=m_sets[0].m_root->volume; + else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + else + bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); + aabbMin=bounds.Mins(); + aabbMax=bounds.Maxs(); +} + +void btDbvtBroadphase::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_prediction = 1/(btScalar)2; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + + m_gid = 0; + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } + } } // @@ -422,9 +600,9 @@ void btDbvtBroadphase::printStats() #if DBVT_BP_ENABLE_BENCHMARK struct btBroadphaseBenchmark - { +{ struct Experiment - { + { const char* name; int object_count; int update_count; @@ -432,109 +610,109 @@ struct btBroadphaseBenchmark int iterations; btScalar speed; btScalar amplitude; - }; + }; struct Object - { + { btVector3 center; btVector3 extents; btBroadphaseProxy* proxy; btScalar time; void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) - { + { time += speed; center[0] = btCos(time*(btScalar)2.17)*amplitude+ - btSin(time)*amplitude/2; + btSin(time)*amplitude/2; center[1] = btCos(time*(btScalar)1.38)*amplitude+ - btSin(time)*amplitude; + btSin(time)*amplitude; center[2] = btSin(time*(btScalar)0.777)*amplitude; pbi->setAabb(proxy,center-extents,center+extents,0); - } - }; + } + }; static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } static void OutputTime(const char* name,btClock& c,unsigned count=0) - { + { const unsigned long us=c.getTimeMicroseconds(); const unsigned long ms=(us+500)/1000; const btScalar sec=us/(btScalar)(1000*1000); if(count>0) printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); - else + else printf("%s : %u us (%u ms)\r\n",name,us,ms); - } - }; + } +}; void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) { -static const btBroadphaseBenchmark::Experiment experiments[]= + 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},*/ + {"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); - srand(180673); - /* Create objects */ - wallclock.reset(); - objects.reserve(object_count); - for(int i=0;i<object_count;++i) + const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp]; + const int object_count=experiment.object_count; + const int update_count=(object_count*experiment.update_count)/100; + const int spawn_count=(object_count*experiment.spawn_count)/100; + const btScalar speed=experiment.speed; + const btScalar amplitude=experiment.amplitude; + printf("Experiment #%u '%s':\r\n",iexp,experiment.name); + printf("\tObjects: %u\r\n",object_count); + printf("\tUpdate: %u\r\n",update_count); + printf("\tSpawn: %u\r\n",spawn_count); + printf("\tSpeed: %f\r\n",speed); + printf("\tAmplitude: %f\r\n",amplitude); + srand(180673); + /* Create objects */ + wallclock.reset(); + objects.reserve(object_count); + for(int i=0;i<object_count;++i) { - btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object(); - po->center[0]=btBroadphaseBenchmark::UnitRand()*50; - po->center[1]=btBroadphaseBenchmark::UnitRand()*50; - po->center[2]=btBroadphaseBenchmark::UnitRand()*50; - po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; - po->time=btBroadphaseBenchmark::UnitRand()*2000; - po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); - objects.push_back(po); + btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object(); + po->center[0]=btBroadphaseBenchmark::UnitRand()*50; + po->center[1]=btBroadphaseBenchmark::UnitRand()*50; + po->center[2]=btBroadphaseBenchmark::UnitRand()*50; + po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; + po->time=btBroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); } - btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); - /* First update */ - wallclock.reset(); - for(int i=0;i<objects.size();++i) + btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); + /* First update */ + wallclock.reset(); + 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 */ - wallclock.reset(); - for(int i=0;i<experiment.iterations;++i) + btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); + /* Updates */ + wallclock.reset(); + for(int i=0;i<experiment.iterations;++i) { - for(int j=0;j<update_count;++j) + for(int j=0;j<update_count;++j) { - objects[j]->update(speed,amplitude,pbi); + objects[j]->update(speed,amplitude,pbi); } - pbi->calculateOverlappingPairs(0); + pbi->calculateOverlappingPairs(0); } - btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); - /* Clean up */ - wallclock.reset(); - for(int i=0;i<objects.size();++i) + btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); + /* Clean up */ + wallclock.reset(); + for(int i=0;i<objects.size();++i) { - pbi->destroyProxy(objects[i]->proxy,0); - delete objects[i]; + pbi->destroyProxy(objects[i]->proxy,0); + delete objects[i]; } - objects.resize(0); - btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + objects.resize(0); + btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); } } @@ -546,3 +724,4 @@ void btDbvtBroadphase::benchmark(btBroadphaseInterface*) #if DBVT_BP_PROFILE #undef SPC #endif + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 1f16043a7a8..fe70bc39c43 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -24,15 +24,15 @@ subject to the following restrictions: // #define DBVT_BP_PROFILE 0 -#define DBVT_BP_SORTPAIRS 1 +//#define DBVT_BP_SORTPAIRS 1 #define DBVT_BP_PREVENTFALSEUPDATE 0 #define DBVT_BP_ACCURATESLEEPING 0 #define DBVT_BP_ENABLE_BENCHMARK 0 #define DBVT_BP_MARGIN (btScalar)0.05 #if DBVT_BP_PROFILE - #define DBVT_BP_PROFILING_RATE 256 - #include "LinearMath/btQuickprof.h" +#define DBVT_BP_PROFILING_RATE 256 +#include "LinearMath/btQuickprof.h" #endif // @@ -40,16 +40,16 @@ subject to the following restrictions: // struct btDbvtProxy : btBroadphaseProxy { -/* Fields */ -btDbvtAabbMm aabb; -btDbvtNode* leaf; -btDbvtProxy* links[2]; -int stage; -/* ctor */ -btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : - btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask) + /* 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) { - links[0]=links[1]=0; + links[0]=links[1]=0; } }; @@ -60,57 +60,67 @@ typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray; ///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. struct btDbvtBroadphase : btBroadphaseInterface { -/* Config */ -enum { + /* 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? #if DBVT_BP_PROFILE -btClock m_clock; -struct { + 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; + } m_profiling; #endif -/* Methods */ -btDbvtBroadphase(btOverlappingPairCache* paircache=0); -~btDbvtBroadphase(); -void collide(btDispatcher* dispatcher); -void optimize(); -/* btBroadphaseInterface Implementation */ -btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); -void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); -void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); -void calculateOverlappingPairs(btDispatcher* dispatcher); -btOverlappingPairCache* getOverlappingPairCache(); -const btOverlappingPairCache* getOverlappingPairCache() const; -void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; -void printStats(); -static void benchmark(btBroadphaseInterface*); + /* Methods */ + btDbvtBroadphase(btOverlappingPairCache* paircache=0); + ~btDbvtBroadphase(); + void collide(btDispatcher* dispatcher); + void optimize(); + /* btBroadphaseInterface Implementation */ + btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); + 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 getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + void calculateOverlappingPairs(btDispatcher* dispatcher); + btOverlappingPairCache* getOverlappingPairCache(); + const btOverlappingPairCache* getOverlappingPairCache() const; + void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; + void printStats(); + static void benchmark(btBroadphaseInterface*); + + + void performDeferredRemoval(btDispatcher* dispatcher); + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + }; #endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 6db71a0170e..ee57aa96151 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -46,22 +46,25 @@ struct btDispatcherInfo m_enableSPU(true), m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), + m_useConvexConservativeDistanceUtil(true), + m_convexConservativeDistanceThreshold(0.0f), m_stackAllocator(0) { } btScalar m_timeStep; - int m_stepCount; - int m_dispatchFunc; + int m_stepCount; + int m_dispatchFunc; mutable btScalar m_timeOfImpact; - bool m_useContinuous; + bool m_useContinuous; class btIDebugDraw* m_debugDraw; - bool m_enableSatConvex; - bool m_enableSPU; - bool m_useEpa; + bool m_enableSatConvex; + bool m_enableSPU; + bool m_useEpa; btScalar m_allowedCcdPenetration; + bool m_useConvexConservativeDistanceUtil; + btScalar m_convexConservativeDistanceThreshold; btStackAlloc* m_stackAllocator; - }; ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp index 3f866ab7c5f..6712f528e97 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -149,6 +149,22 @@ 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) @@ -208,7 +224,9 @@ void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab - m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + if (m_optimizedAabbTree) + m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + int i; for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) @@ -464,3 +482,8 @@ void btMultiSapBroadphase::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 index a0c002de856..91c504eee22 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -26,6 +26,7 @@ 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. @@ -72,7 +73,7 @@ public: short int m_collisionFilterMask; */ btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) - :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), + :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask), m_aabbMin(aabbMin), m_aabbMax(aabbMax), m_shapeType(shapeType) @@ -108,6 +109,9 @@ public: 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); @@ -139,6 +143,9 @@ public: 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 ff65cdde79f..b209bcb9a20 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: #include "btDispatcher.h" #include "btCollisionAlgorithm.h" +#include "LinearMath/btAabbUtil2.h" #include <stdio.h> @@ -33,7 +34,8 @@ int gFindPairs =0; btHashedOverlappingPairCache::btHashedOverlappingPairCache(): m_overlapFilterCallback(0), - m_blockedForChanges(false) + m_blockedForChanges(false), + m_ghostPairCallback(0) { int initialAllocatedSize= 2; m_overlappingPairArray.reserve(initialAllocatedSize); @@ -45,7 +47,6 @@ btHashedOverlappingPairCache::btHashedOverlappingPairCache(): btHashedOverlappingPairCache::~btHashedOverlappingPairCache() { - //todo/test: show we erase/delete data, or is it automatic } @@ -135,7 +136,8 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { gFindPairs++; - if(proxy0>proxy1) btSwap(proxy0,proxy1); + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); @@ -211,7 +213,8 @@ void btHashedOverlappingPairCache::growTables() btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - if(proxy0>proxy1) btSwap(proxy0,proxy1); + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); @@ -238,6 +241,11 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx int count = m_overlappingPairArray.size(); int oldCapacity = m_overlappingPairArray.capacity(); void* mem = &m_overlappingPairArray.expand(); + + //this is where we add an actual pair, so also call the 'ghost' + if (m_ghostPairCallback) + m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); + int newCapacity = m_overlappingPairArray.capacity(); if (oldCapacity < newCapacity) @@ -251,7 +259,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx // pair->m_pProxy0 = proxy0; // pair->m_pProxy1 = proxy1; pair->m_algorithm = 0; - pair->m_userInfo = 0; + pair->m_internalTmpValue = 0; m_next[count] = m_hashTable[hash]; @@ -265,7 +273,8 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) { gRemovePairs++; - if(proxy0>proxy1) btSwap(proxy0,proxy1); + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); @@ -282,7 +291,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro cleanOverlappingPair(*pair,dispatcher); - void* userData = pair->m_userInfo; + void* userData = pair->m_internalInfo1; btAssert(pair->m_pProxy0->getUid() == proxyId1); btAssert(pair->m_pProxy1->getUid() == proxyId2); @@ -317,6 +326,9 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro int lastPairIndex = m_overlappingPairArray.size() - 1; + if (m_ghostPairCallback) + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + // If the removed pair is the last pair, we are done. if (lastPairIndex == pairIndex) { @@ -384,6 +396,35 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* } } +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++) + { + tmpPairs.push_back(m_overlappingPairArray[i]); + } + + for (i=0;i<tmpPairs.size();i++) + { + removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher); + } + + for (i = 0; i < m_next.size(); i++) + { + m_next[i] = BT_NULL_PAIR; + } + + tmpPairs.quickSort(btBroadphasePairSortPredicate()); + + for (i=0;i<tmpPairs.size();i++) + { + addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1); + } + + +} void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher ) @@ -397,8 +438,10 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro { gOverlappingPairs--; btBroadphasePair& pair = m_overlappingPairArray[findIndex]; - void* userData = pair.m_userInfo; + void* userData = pair.m_internalInfo1; cleanOverlappingPair(pair,dispatcher); + if (m_ghostPairCallback) + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); m_overlappingPairArray.pop_back(); @@ -419,15 +462,19 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) { //don't add overlap with own - assert(proxy0 != proxy1); + btAssert(proxy0 != proxy1); if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; void* mem = &m_overlappingPairArray.expand(); btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); + gOverlappingPairs++; gAddedPairs++; + + if (m_ghostPairCallback) + m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); return pair; } @@ -446,7 +493,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP if (findIndex < m_overlappingPairArray.size()) { - //assert(it != m_overlappingPairSet.end()); + //btAssert(it != m_overlappingPairSet.end()); btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; return pair; } @@ -476,8 +523,9 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* if (callback->processOverlap(*pair)) { cleanOverlappingPair(*pair,dispatcher); - - m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1); + pair->m_pProxy0 = 0; + pair->m_pProxy1 = 0; + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); m_overlappingPairArray.pop_back(); gOverlappingPairs--; } else @@ -493,7 +541,8 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* btSortedOverlappingPairCache::btSortedOverlappingPairCache(): m_blockedForChanges(false), m_hasDeferredRemoval(true), - m_overlapFilterCallback(0) + m_overlapFilterCallback(0), + m_ghostPairCallback(0) { int initialAllocatedSize= 2; m_overlappingPairArray.reserve(initialAllocatedSize); @@ -501,7 +550,6 @@ btSortedOverlappingPairCache::btSortedOverlappingPairCache(): btSortedOverlappingPairCache::~btSortedOverlappingPairCache() { - //todo/test: show we erase/delete data, or is it automatic } void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) @@ -577,3 +625,9 @@ void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad processAllOverlappingPairs(&removeCallback,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 66679bd218a..eda45c47b5b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -21,7 +21,6 @@ subject to the following restrictions: #include "btBroadphaseProxy.h" #include "btOverlappingPairCallback.h" -#include "LinearMath/btPoint3.h" #include "LinearMath/btAlignedObjectArray.h" class btDispatcher; @@ -83,6 +82,11 @@ public: 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 @@ -253,10 +257,19 @@ private: return false; } -public: + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher); + + +protected: btAlignedObjectArray<int> m_hashTable; btAlignedObjectArray<int> m_next; + btOverlappingPairCallback* m_ghostPairCallback; }; @@ -280,6 +293,8 @@ class btSortedOverlappingPairCache : public btOverlappingPairCache //if set, use the callback instead of the built in filter in needBroadphaseCollision btOverlapFilterCallback* m_overlapFilterCallback; + btOverlappingPairCallback* m_ghostPairCallback; + public: btSortedOverlappingPairCache(); @@ -355,12 +370,19 @@ class btSortedOverlappingPairCache : public btOverlappingPairCache 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 testing. +///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. class btNullPairCache : public btOverlappingPairCache { @@ -414,6 +436,11 @@ public: return true; } + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) + { + + } + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) { return 0; @@ -427,6 +454,10 @@ public: virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) { } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) + { + } }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index a30bd1fd9e1..8bef8f0d43e 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -18,14 +18,18 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btIDebugDraw.h" +#define RAYAABB2 -btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), +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); } @@ -119,7 +123,7 @@ void btQuantizedBvh::buildTree (int startIndex,int endIndex) int numIndices =endIndex-startIndex; int curIndex = m_curNodeIndex; - assert(numIndices>0); + btAssert(numIndices>0); if (numIndices==1) { @@ -140,8 +144,11 @@ void btQuantizedBvh::buildTree (int startIndex,int endIndex) int internalNodeIndex = m_curNodeIndex; - setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin); - setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax); + //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++) { @@ -177,6 +184,9 @@ void btQuantizedBvh::buildTree (int startIndex,int endIndex) { updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); } + } else + { + } setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); @@ -338,6 +348,7 @@ void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb int maxIterations = 0; + void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const { btAssert(!m_useQuantization); @@ -352,7 +363,7 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const while (curIndex < m_curNodeIndex) { //catch bugs in tree data - assert (walkIterations < m_curNodeIndex); + btAssert (walkIterations < m_curNodeIndex); walkIterations++; aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); @@ -434,6 +445,96 @@ void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize +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); + + const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0]; + int escapeIndex, curIndex = 0; + int walkIterations = 0; + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap=0; + unsigned rayBoxOverlap=0; + btScalar lambda_max = 1.0; + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + +#ifdef RAYAABB2 + btVector3 rayDir = (rayTarget-raySource); + rayDir.normalize (); + 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(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; +#endif + + btVector3 bounds[2]; + + while (curIndex < m_curNodeIndex) + { + btScalar param = 1.0; + //catch bugs in tree data + btAssert (walkIterations < m_curNodeIndex); + + walkIterations++; + + bounds[0] = rootNode->m_aabbMinOrg; + bounds[1] = rootNode->m_aabbMaxOrg; + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; + + 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; + +#else + btVector3 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); + } + + //PCK: unsigned instead of bool + if ((rayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } 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 @@ -454,9 +555,8 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* unsigned rayBoxOverlap = 0; btScalar lambda_max = 1.0; -#define RAYAABB2 + #ifdef RAYAABB2 - btVector3 rayFrom = raySource; btVector3 rayDirection = (rayTarget-raySource); rayDirection.normalize (); lambda_max = rayDirection.dot(rayTarget-raySource); @@ -502,7 +602,7 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* #endif//VISUALLY_ANALYZE_BVH //catch bugs in tree data - assert (walkIterations < subTreeSize); + btAssert (walkIterations < subTreeSize); walkIterations++; //PCK: unsigned instead of bool @@ -533,7 +633,9 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* ///thanks Joerg/hiker for the reproduction case! ///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); + #else rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); #endif @@ -597,7 +699,7 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb #endif//VISUALLY_ANALYZE_BVH //catch bugs in tree data - assert (walkIterations < subTreeSize); + btAssert (walkIterations < subTreeSize); walkIterations++; //PCK: unsigned instead of bool @@ -652,30 +754,25 @@ void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallba void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const { - bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; - if (fast_path) - { - walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex); - } else { - /* Otherwise fallback to AABB overlap test */ - btVector3 aabbMin = raySource; - btVector3 aabbMax = raySource; - aabbMin.setMin(rayTarget); - aabbMax.setMax(rayTarget); - reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax); - } + 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 { - bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; - if (fast_path) + //always use stackless + + if (m_useQuantization) { walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); - } else { - /* Slow path: - Construct the bounding box for the entire box cast and send that down the tree */ + } + else + { + walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } + /* + { + //recursive traversal btVector3 qaabbMin = raySource; btVector3 qaabbMax = raySource; qaabbMin.setMin(rayTarget); @@ -684,6 +781,8 @@ void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCa qaabbMax += aabbMax; reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); } + */ + } @@ -716,17 +815,19 @@ void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNod //PCK: include #include <new> +#if 0 //PCK: consts static const unsigned BVH_ALIGNMENT = 16; static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; static const unsigned BVH_ALIGNMENT_BLOCKS = 2; - +#endif unsigned int btQuantizedBvh::getAlignmentSerializationPadding() { - return BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; + // I changed this to 0 since the extra padding is not needed or used. + return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; } unsigned btQuantizedBvh::calculateSerializeBufferSize() @@ -742,7 +843,7 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize() bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) { - assert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); + 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)) @@ -829,6 +930,11 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe } } nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(NULL, 0, 0); } else { @@ -859,6 +965,11 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe } } nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0); } sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; @@ -896,12 +1007,23 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex); targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize); - targetBvh->m_SubtreeHeaders[i] = m_SubtreeHeaders[i]; + + // need to clear padding in destination buffer + targetBvh->m_SubtreeHeaders[i].m_padding[0] = 0; + targetBvh->m_SubtreeHeaders[i].m_padding[1] = 0; + targetBvh->m_SubtreeHeaders[i].m_padding[2] = 0; } } - nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_SubtreeHeaders.initializeFromBuffer(NULL, 0, 0); + + // this wipes the virtual function table pointer at the start of the buffer for the class + *((void**)o_alignedDataBuffer) = NULL; + return true; } @@ -1015,11 +1137,12 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : m_bvhAabbMin(self.m_bvhAabbMin), m_bvhAabbMax(self.m_bvhAabbMax), -m_bvhQuantization(self.m_bvhQuantization) +m_bvhQuantization(self.m_bvhQuantization), +m_bulletVersion(BT_BULLET_VERSION) { - } + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index 8a149b533fa..ced457b6036 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -158,41 +158,43 @@ typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray; ///It is recommended to use quantization for better performance and lower memory requirements. ATTRIBUTE_ALIGNED16(class) btQuantizedBvh { -protected: - - NodeArray m_leafNodes; - NodeArray m_contiguousNodes; - - QuantizedNodeArray m_quantizedLeafNodes; - - QuantizedNodeArray m_quantizedContiguousNodes; - - int m_curNodeIndex; - - - //quantization data - bool m_useQuantization; - btVector3 m_bvhAabbMin; - btVector3 m_bvhAabbMax; - btVector3 m_bvhQuantization; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - enum btTraversalMode { TRAVERSAL_STACKLESS = 0, TRAVERSAL_STACKLESS_CACHE_FRIENDLY, TRAVERSAL_RECURSIVE }; + protected: - btTraversalMode m_traversalMode; + + 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; + //quantization data + bool m_useQuantization; + + + + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + QuantizedNodeArray m_quantizedLeafNodes; + QuantizedNodeArray m_quantizedContiguousNodes; + btTraversalMode m_traversalMode; BvhSubtreeInfoArray m_SubtreeHeaders; //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray int m_subtreeHeaderCount; + + + ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) ///this might be refactored into a virtual, it is usually not calculated at run-time @@ -296,6 +298,7 @@ protected: 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; @@ -307,30 +310,14 @@ protected: void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; -#define USE_BANCHLESS 1 -#ifdef USE_BANCHLESS - //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) - SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const - { - return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) - & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) - & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), - 1, 0)); - } -#else - SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const - { - bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } -#endif //USE_BANCHLESS + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + btQuantizedBvh(); virtual ~btQuantizedBvh(); @@ -363,7 +350,7 @@ public: btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) - ///todo: double-check this + ///@todo: double-check this if (isMax) { out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index a57952ffa06..caed63db005 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -55,6 +55,7 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o m_maxHandles = maxProxies; m_numHandles = 0; m_firstFreeHandle = 0; + m_LastHandleIndex = -1; { @@ -88,7 +89,7 @@ btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, btAssert(0); return 0; //should never happen, but don't let the game crash ;-) } - assert(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); @@ -137,14 +138,32 @@ void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* } +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*/) { btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); - sbp->m_min = aabbMin; - sbp->m_max = aabbMax; + 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) +{ + for (int i=0; i <= m_LastHandleIndex; i++) + { + btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; + if(!proxy->m_clientObject) + { + continue; + } + rayCallback.process(proxy); + } +} @@ -154,9 +173,9 @@ void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbM bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1) { - return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] && - proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] && - proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2]; + 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]; } @@ -176,18 +195,25 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { //first check for new overlapping pairs int i,j; - if (m_numHandles >= 0) { - - for (i=0;i<m_numHandles;i++) + int new_largest_index = -1; + for (i=0; i <= m_LastHandleIndex; i++) { btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i]; - - for (j=i+1;j<m_numHandles;j++) + if(!proxy0->m_clientObject) + { + continue; + } + new_largest_index = i; + for (j=i+1; j <= m_LastHandleIndex; j++) { btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j]; btAssert(proxy0 != proxy1); + if(!proxy1->m_clientObject) + { + continue; + } btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); @@ -211,6 +237,8 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) } } + m_LastHandleIndex = new_largest_index; + if (m_ownsPairCache && m_pairCache->hasDeferredRemoval()) { @@ -296,5 +324,7 @@ bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseP return aabbOverlap(p0,p1); } - - +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 e2ebb825725..cc7613bf6a0 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -22,8 +22,6 @@ subject to the following restrictions: struct btSimpleBroadphaseProxy : public btBroadphaseProxy { - btVector3 m_min; - btVector3 m_max; int m_nextFree; // int m_handleId; @@ -31,9 +29,8 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy btSimpleBroadphaseProxy() {}; - btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy) - :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy), - m_min(minpt),m_max(maxpt) + 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) { (void)shapeType; } @@ -56,6 +53,7 @@ protected: int m_numHandles; // number of active handles int m_maxHandles; // max number of handles + int m_LastHandleIndex; btSimpleBroadphaseProxy* m_pHandles; // handles pool @@ -68,6 +66,10 @@ protected: int freeHandle = m_firstFreeHandle; m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree(); m_numHandles++; + if(freeHandle > m_LastHandleIndex) + { + m_LastHandleIndex = freeHandle; + } return freeHandle; } @@ -75,10 +77,15 @@ protected: { int handle = int(proxy-m_pHandles); btAssert(handle >= 0 && handle < m_maxHandles); - + if(handle == m_LastHandleIndex) + { + m_LastHandleIndex--; + } proxy->SetNextFree(m_firstFreeHandle); m_firstFreeHandle = handle; + proxy->m_clientObject = 0; + m_numHandles--; } @@ -95,6 +102,15 @@ protected: return proxy0; } + inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const + { + const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy); + return proxy0; + } + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + void validate(); @@ -117,6 +133,9 @@ public: 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)); btOverlappingPairCache* getOverlappingPairCache() { diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt index d77ca6444c7..4b4304f43b0 100644 --- a/extern/bullet2/src/BulletCollision/CMakeLists.txt +++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt @@ -1,153 +1,234 @@ - -INCLUDE_DIRECTORIES( -${BULLET_PHYSICS_SOURCE_DIR}/src } +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } ) + +SET(BulletCollision_SRCS + BroadphaseCollision/btAxisSweep3.cpp + BroadphaseCollision/btBroadphaseProxy.cpp + BroadphaseCollision/btCollisionAlgorithm.cpp + BroadphaseCollision/btDispatcher.cpp + BroadphaseCollision/btDbvtBroadphase.cpp + BroadphaseCollision/btDbvt.cpp + BroadphaseCollision/btMultiSapBroadphase.cpp + BroadphaseCollision/btOverlappingPairCache.cpp + BroadphaseCollision/btQuantizedBvh.cpp + BroadphaseCollision/btSimpleBroadphase.cpp + CollisionDispatch/btActivatingCollisionAlgorithm.cpp + CollisionDispatch/btCollisionDispatcher.cpp + CollisionDispatch/btCollisionObject.cpp + CollisionDispatch/btCollisionWorld.cpp + CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btDefaultCollisionConfiguration.cpp + CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxDetector.cpp + CollisionDispatch/btGhostObject.cpp + CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp + CollisionDispatch/btConvexConvexAlgorithm.cpp + CollisionDispatch/btEmptyCollisionAlgorithm.cpp + CollisionDispatch/btManifoldResult.cpp + CollisionDispatch/btSimulationIslandManager.cpp + CollisionDispatch/btUnionFind.cpp + CollisionDispatch/SphereTriangleDetector.cpp + CollisionShapes/btBoxShape.cpp + CollisionShapes/btBvhTriangleMeshShape.cpp + CollisionShapes/btCapsuleShape.cpp + CollisionShapes/btCollisionShape.cpp + CollisionShapes/btCompoundShape.cpp + CollisionShapes/btConcaveShape.cpp + CollisionShapes/btConeShape.cpp + CollisionShapes/btConvexHullShape.cpp + CollisionShapes/btConvexPointCloudShape.cpp + CollisionShapes/btConvexShape.cpp + CollisionShapes/btConvexInternalShape.cpp + CollisionShapes/btConvexTriangleMeshShape.cpp + CollisionShapes/btCylinderShape.cpp + CollisionShapes/btEmptyShape.cpp + CollisionShapes/btHeightfieldTerrainShape.cpp + CollisionShapes/btMinkowskiSumShape.cpp + CollisionShapes/btMultimaterialTriangleMeshShape.cpp + CollisionShapes/btMultiSphereShape.cpp + CollisionShapes/btOptimizedBvh.cpp + CollisionShapes/btPolyhedralConvexShape.cpp + CollisionShapes/btScaledBvhTriangleMeshShape.cpp + CollisionShapes/btTetrahedronShape.cpp + CollisionShapes/btSphereShape.cpp + CollisionShapes/btShapeHull.cpp + CollisionShapes/btStaticPlaneShape.cpp + CollisionShapes/btStridingMeshInterface.cpp + CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleBuffer.cpp + CollisionShapes/btTriangleIndexVertexArray.cpp + CollisionShapes/btTriangleIndexVertexMaterialArray.cpp + CollisionShapes/btTriangleMesh.cpp + CollisionShapes/btTriangleMeshShape.cpp + CollisionShapes/btUniformScalingShape.cpp + Gimpact/btContactProcessing.cpp + Gimpact/btGImpactShape.cpp + Gimpact/gim_contact.cpp + Gimpact/btGImpactBvh.cpp + Gimpact/btGenericPoolAllocator.cpp + Gimpact/gim_memory.cpp + Gimpact/btGImpactCollisionAlgorithm.cpp + Gimpact/btTriangleShapeEx.cpp + Gimpact/gim_tri_collision.cpp + Gimpact/btGImpactQuantizedBvh.cpp + Gimpact/gim_box_set.cpp + NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btGjkEpa2.cpp + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp + NarrowPhaseCollision/btConvexCast.cpp + NarrowPhaseCollision/btGjkConvexCast.cpp + NarrowPhaseCollision/btGjkPairDetector.cpp + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp + NarrowPhaseCollision/btPersistentManifold.cpp + NarrowPhaseCollision/btRaycastCallback.cpp + NarrowPhaseCollision/btSubSimplexConvexCast.cpp + NarrowPhaseCollision/btVoronoiSimplexSolver.cpp ) - -ADD_LIBRARY(LibBulletCollision - BroadphaseCollision/btAxisSweep3.cpp - BroadphaseCollision/btAxisSweep3.h - BroadphaseCollision/btBroadphaseProxy.cpp - BroadphaseCollision/btBroadphaseProxy.h - BroadphaseCollision/btCollisionAlgorithm.cpp - BroadphaseCollision/btCollisionAlgorithm.h - BroadphaseCollision/btDispatcher.cpp - BroadphaseCollision/btDispatcher.h - BroadphaseCollision/btDbvtBroadphase.cpp - BroadphaseCollision/btDbvtBroadphase.h - BroadphaseCollision/btDbvt.cpp - BroadphaseCollision/btDbvt.h - BroadphaseCollision/btMultiSapBroadphase.cpp - BroadphaseCollision/btMultiSapBroadphase.h - BroadphaseCollision/btOverlappingPairCache.cpp - BroadphaseCollision/btOverlappingPairCache.h - BroadphaseCollision/btOverlappingPairCallback.h - BroadphaseCollision/btQuantizedBvh.cpp - BroadphaseCollision/btQuantizedBvh.h - BroadphaseCollision/btSimpleBroadphase.cpp - BroadphaseCollision/btSimpleBroadphase.h - CollisionDispatch/btCollisionDispatcher.cpp - CollisionDispatch/btCollisionDispatcher.h - CollisionDispatch/btCollisionObject.cpp - CollisionDispatch/btCollisionObject.h - CollisionDispatch/btCollisionWorld.cpp - CollisionDispatch/btCollisionWorld.h - CollisionDispatch/btCompoundCollisionAlgorithm.cpp - CollisionDispatch/btCompoundCollisionAlgorithm.h - CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp - CollisionDispatch/btConvexConcaveCollisionAlgorithm.h - CollisionDispatch/btDefaultCollisionConfiguration.cpp - CollisionDispatch/btDefaultCollisionConfiguration.h - CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp - CollisionDispatch/btSphereSphereCollisionAlgorithm.h - CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp - CollisionDispatch/btBoxBoxCollisionAlgorithm.h - CollisionDispatch/btBoxBoxDetector.cpp - CollisionDispatch/btBoxBoxDetector.h - CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp - CollisionDispatch/btSphereBoxCollisionAlgorithm.h - CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp - CollisionDispatch/btConvexPlaneCollisionAlgorithm.h - CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp - CollisionDispatch/btSphereTriangleCollisionAlgorithm.h - CollisionDispatch/btConvexConvexAlgorithm.cpp - CollisionDispatch/btConvexConvexAlgorithm.h - CollisionDispatch/btEmptyCollisionAlgorithm.cpp - CollisionDispatch/btEmptyCollisionAlgorithm.h - CollisionDispatch/btManifoldResult.cpp - CollisionDispatch/btManifoldResult.h - CollisionDispatch/btSimulationIslandManager.cpp - CollisionDispatch/btSimulationIslandManager.h - CollisionDispatch/btUnionFind.cpp - CollisionDispatch/btUnionFind.h - CollisionDispatch/SphereTriangleDetector.cpp - CollisionDispatch/SphereTriangleDetector.h - CollisionShapes/btBoxShape.cpp - CollisionShapes/btBoxShape.h - CollisionShapes/btBvhTriangleMeshShape.cpp - CollisionShapes/btBvhTriangleMeshShape.h - CollisionShapes/btCapsuleShape.cpp - CollisionShapes/btCapsuleShape.h - CollisionShapes/btCollisionShape.cpp - CollisionShapes/btCollisionShape.h - CollisionShapes/btCompoundShape.cpp - CollisionShapes/btCompoundShape.h - CollisionShapes/btConcaveShape.cpp - CollisionShapes/btConcaveShape.h - CollisionShapes/btConeShape.cpp - CollisionShapes/btConeShape.h - CollisionShapes/btConvexHullShape.cpp - CollisionShapes/btConvexHullShape.h - CollisionShapes/btConvexShape.cpp - CollisionShapes/btConvexShape.h - CollisionShapes/btConvexInternalShape.cpp - CollisionShapes/btConvexInternalShape.h - CollisionShapes/btConvexTriangleMeshShape.cpp - CollisionShapes/btConvexTriangleMeshShape.h - CollisionShapes/btCylinderShape.cpp - CollisionShapes/btCylinderShape.h - CollisionShapes/btEmptyShape.cpp - CollisionShapes/btEmptyShape.h - CollisionShapes/btHeightfieldTerrainShape.cpp - CollisionShapes/btHeightfieldTerrainShape.h - CollisionShapes/btMinkowskiSumShape.cpp - CollisionShapes/btMinkowskiSumShape.h - CollisionShapes/btMaterial.h - CollisionShapes/btMultimaterialTriangleMeshShape.cpp - CollisionShapes/btMultimaterialTriangleMeshShape.h - CollisionShapes/btMultiSphereShape.cpp - CollisionShapes/btMultiSphereShape.h - CollisionShapes/btOptimizedBvh.cpp - CollisionShapes/btOptimizedBvh.h - CollisionShapes/btPolyhedralConvexShape.cpp - CollisionShapes/btPolyhedralConvexShape.h - CollisionShapes/btScaledBvhTriangleMeshShape.cpp - CollisionShapes/btScaledBvhTriangleMeshShape.h - CollisionShapes/btTetrahedronShape.cpp - CollisionShapes/btTetrahedronShape.h - CollisionShapes/btSphereShape.cpp - CollisionShapes/btSphereShape.h - CollisionShapes/btShapeHull.h - CollisionShapes/btShapeHull.cpp - CollisionShapes/btStaticPlaneShape.cpp - CollisionShapes/btStaticPlaneShape.h - CollisionShapes/btStridingMeshInterface.cpp - CollisionShapes/btStridingMeshInterface.h - CollisionShapes/btTriangleCallback.cpp - CollisionShapes/btTriangleCallback.h - CollisionShapes/btTriangleBuffer.cpp - CollisionShapes/btTriangleBuffer.h - CollisionShapes/btTriangleIndexVertexArray.cpp - CollisionShapes/btTriangleIndexVertexArray.h - CollisionShapes/btTriangleIndexVertexMaterialArray.h - CollisionShapes/btTriangleIndexVertexMaterialArray.cpp - CollisionShapes/btTriangleMesh.cpp - CollisionShapes/btTriangleMesh.h - CollisionShapes/btTriangleMeshShape.cpp - CollisionShapes/btTriangleMeshShape.h - CollisionShapes/btUniformScalingShape.cpp - CollisionShapes/btUniformScalingShape.h - NarrowPhaseCollision/btContinuousConvexCollision.cpp - NarrowPhaseCollision/btContinuousConvexCollision.h - NarrowPhaseCollision/btGjkEpa.cpp - NarrowPhaseCollision/btGjkEpa.h - NarrowPhaseCollision/btGjkEpa2.cpp - NarrowPhaseCollision/btGjkEpa2.h - NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp - NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h - NarrowPhaseCollision/btConvexCast.cpp - NarrowPhaseCollision/btConvexCast.h - NarrowPhaseCollision/btGjkConvexCast.cpp - NarrowPhaseCollision/btGjkConvexCast.h - NarrowPhaseCollision/btGjkPairDetector.cpp - NarrowPhaseCollision/btGjkPairDetector.h - NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp - NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h - NarrowPhaseCollision/btPersistentManifold.cpp - NarrowPhaseCollision/btPersistentManifold.h - NarrowPhaseCollision/btRaycastCallback.cpp - NarrowPhaseCollision/btRaycastCallback.h - NarrowPhaseCollision/btSubSimplexConvexCast.cpp - NarrowPhaseCollision/btSubSimplexConvexCast.h - NarrowPhaseCollision/btVoronoiSimplexSolver.cpp - NarrowPhaseCollision/btVoronoiSimplexSolver.h + +SET(Root_HDRS + ../btBulletCollisionCommon.h ) +SET(BroadphaseCollision_HDRS + BroadphaseCollision/btAxisSweep3.h + BroadphaseCollision/btBroadphaseInterface.h + BroadphaseCollision/btBroadphaseProxy.h + BroadphaseCollision/btCollisionAlgorithm.h + BroadphaseCollision/btDispatcher.h + BroadphaseCollision/btDbvtBroadphase.h + BroadphaseCollision/btDbvt.h + BroadphaseCollision/btMultiSapBroadphase.h + BroadphaseCollision/btOverlappingPairCache.h + BroadphaseCollision/btOverlappingPairCallback.h + BroadphaseCollision/btQuantizedBvh.h + BroadphaseCollision/btSimpleBroadphase.h +) +SET(CollisionDispatch_HDRS + CollisionDispatch/btActivatingCollisionAlgorithm.h + CollisionDispatch/btCollisionConfiguration.h + CollisionDispatch/btCollisionCreateFunc.h + CollisionDispatch/btCollisionDispatcher.h + CollisionDispatch/btCollisionObject.h + CollisionDispatch/btCollisionWorld.h + CollisionDispatch/btCompoundCollisionAlgorithm.h + CollisionDispatch/btConvexConcaveCollisionAlgorithm.h + CollisionDispatch/btDefaultCollisionConfiguration.h + CollisionDispatch/btSphereSphereCollisionAlgorithm.h + CollisionDispatch/btBoxBoxCollisionAlgorithm.h + CollisionDispatch/btBoxBoxDetector.h + CollisionDispatch/btGhostObject.h + CollisionDispatch/btSphereBoxCollisionAlgorithm.h + CollisionDispatch/btConvexPlaneCollisionAlgorithm.h + CollisionDispatch/btSphereTriangleCollisionAlgorithm.h + CollisionDispatch/btConvexConvexAlgorithm.h + CollisionDispatch/btEmptyCollisionAlgorithm.h + CollisionDispatch/btManifoldResult.h + CollisionDispatch/btSimulationIslandManager.h + CollisionDispatch/btUnionFind.h + CollisionDispatch/SphereTriangleDetector.h +) +SET(CollisionShapes_HDRS + CollisionShapes/btBoxShape.h + CollisionShapes/btBvhTriangleMeshShape.h + CollisionShapes/btCapsuleShape.h + CollisionShapes/btCollisionMargin + CollisionShapes/btCollisionShape.h + CollisionShapes/btCompoundShape.h + CollisionShapes/btConcaveShape.h + CollisionShapes/btConeShape.h + CollisionShapes/btConvexHullShape.h + CollisionShapes/btConvexPointCloudShape.h + CollisionShapes/btConvexShape.h + CollisionShapes/btConvexInternalShape.h + CollisionShapes/btConvexTriangleMeshShape.h + CollisionShapes/btCylinderShape.h + CollisionShapes/btEmptyShape.h + CollisionShapes/btHeightfieldTerrainShape.h + CollisionShapes/btMinkowskiSumShape.h + CollisionShapes/btMaterial.h + CollisionShapes/btMultimaterialTriangleMeshShape.h + CollisionShapes/btMultiSphereShape.h + CollisionShapes/btOptimizedBvh.h + CollisionShapes/btPolyhedralConvexShape.h + CollisionShapes/btScaledBvhTriangleMeshShape.h + CollisionShapes/btTetrahedronShape.h + CollisionShapes/btSphereShape.h + CollisionShapes/btShapeHull.h + CollisionShapes/btStaticPlaneShape.h + CollisionShapes/btStridingMeshInterface.h + CollisionShapes/btTriangleCallback.h + CollisionShapes/btTriangleBuffer.h + CollisionShapes/btTriangleIndexVertexArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.h + CollisionShapes/btTriangleMesh.h + CollisionShapes/btTriangleMeshShape.h + CollisionShapes/btUniformScalingShape.h +) +SET(Gimpact_HDRS + Gimpact/btGImpactShape.h + Gimpact/gim_contact.h + Gimpact/btGImpactBvh.h + Gimpact/btGenericPoolAllocator.h + Gimpact/gim_memory.h + Gimpact/btGImpactCollisionAlgorithm.h + Gimpact/btTriangleShapeEx.h + Gimpact/gim_tri_collision.h + Gimpact/btGImpactQuantizedBvh.h + Gimpact/gim_box_set.h +) +SET(NarrowPhaseCollision_HDRS + NarrowPhaseCollision/btContinuousConvexCollision.h + NarrowPhaseCollision/btConvexCast.h + NarrowPhaseCollision/btConvexPenetrationDepthSolver.h + NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h + NarrowPhaseCollision/btGjkConvexCast.h + NarrowPhaseCollision/btGjkEpa2.h + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h + NarrowPhaseCollision/btGjkPairDetector.h + NarrowPhaseCollision/btManifoldPoint.h + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h + NarrowPhaseCollision/btPersistentManifold.h + NarrowPhaseCollision/btPointCollector.h + NarrowPhaseCollision/btRaycastCallback.h + NarrowPhaseCollision/btSimplexSolverInterface.h + NarrowPhaseCollision/btSubSimplexConvexCast.h + NarrowPhaseCollision/btVoronoiSimplexSolver.h +) + +SET(BulletCollision_HDRS + ${Root_HDRS} + ${BroadphaseCollision_HDRS} + ${CollisionDispatch_HDRS} + ${CollisionShapes_HDRS} + ${Gimpact_HDRS} + ${NarrowPhaseCollision_HDRS} +) + + +ADD_LIBRARY(BulletCollision ${BulletCollision_SRCS} ${BulletCollision_HDRS}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletCollision LinearMath) +ENDIF (BUILD_SHARED_LIBS) + +#INSTALL of other files requires CMake 2.6 +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS BulletCollision DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES FRAMEWORK true) + + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${BroadphaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadphaseCollision) + SET_PROPERTY(SOURCE ${CollisionDispatch_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionDispatch) + SET_PROPERTY(SOURCE ${CollisionShapes_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionShapes) + SET_PROPERTY(SOURCE ${Gimpact_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Gimpact) + SET_PROPERTY(SOURCE ${NarrowPhaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/NarrowPhaseCollision) + +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index f6c1e32ac7c..9a749a03793 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -19,9 +19,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btSphereShape.h" -SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle) +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) :m_sphere(sphere), -m_triangle(triangle) +m_triangle(triangle), +m_contactBreakingThreshold(contactBreakingThreshold) { } @@ -40,7 +41,7 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res //move sphere into triangle space btTransform sphereInTr = transformB.inverseTimes(transformA); - if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact)) + if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) { if (swapResults) { @@ -93,7 +94,7 @@ bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* ve } ///combined discrete/continuous sphere-triangle -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact) +bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) { const btVector3* vertices = &m_triangle->getVertexPtr(0); @@ -115,10 +116,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po normal *= btScalar(-1.); } - ///todo: move this gContactBreakingThreshold into a proper structure - extern btScalar gContactBreakingThreshold; - - btScalar contactMargin = gContactBreakingThreshold; + btScalar contactMargin = contactBreakingThreshold; bool isInsideContactPlane = distanceFromPlane < r + contactMargin; bool isInsideShellPlane = distanceFromPlane < r; @@ -140,8 +138,8 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po btVector3 nearestOnEdge; for (int i = 0; i < m_triangle->getNumEdges(); i++) { - btPoint3 pa; - btPoint3 pb; + btVector3 pa; + btVector3 pb; m_triangle->getEdge(i,pa,pb); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 26dabaa480e..981bd54e76c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define SPHERE_TRIANGLE_DETECTOR_H #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" + class btSphereShape; @@ -30,19 +30,19 @@ struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface { virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); - SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle); + SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold); virtual ~SphereTriangleDetector() {}; private: - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact); + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); 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; }; #endif //SPHERE_TRIANGLE_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp new file mode 100644 index 00000000000..7e5da6c5872 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btActivatingCollisionAlgorithm.h" +#include "btCollisionDispatcher.h" +#include "btCollisionObject.h" + +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +} +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1) +: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(); +// } +} + +btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() +{ +// m_colObj0->activate(); +// m_colObj1->activate(); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h new file mode 100644 index 00000000000..25fe088942d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -0,0 +1,36 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H +#define __BT_ACTIVATING_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +///This class is not enabled yet (work-in-progress) to more aggressively activate objects. +class btActivatingCollisionAlgorithm : public btCollisionAlgorithm +{ +// btCollisionObject* m_colObj0; +// btCollisionObject* m_colObj1; + +public: + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1); + + virtual ~btActivatingCollisionAlgorithm(); + +}; +#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index cd0c028012c..d3342c547b5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,obj0,obj1), m_ownManifold(false), m_manifoldPtr(mf) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index 35afaf175a1..e7d2cc25c22 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BOX_BOX__COLLISION_ALGORITHM_H #define BOX_BOX__COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" @@ -24,14 +24,14 @@ subject to the following restrictions: class btPersistentManifold; ///box-box collision detection -class btBoxBoxCollisionAlgorithm : public btCollisionAlgorithm +class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; public: btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} + : btActivatingCollisionAlgorithm(ci) {} virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 45ebff5dc45..31353f1b2c4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -207,7 +207,13 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) cy += q*(p[i*2+1]+p[i*2+3]); } q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - a = 1.f/(btScalar(3.0)*(a+q)); + if (btFabs(a+q) > SIMD_EPSILON) + { + a = 1.f/(btScalar(3.0)*(a+q)); + } else + { + a=1e30f; + } cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); } @@ -226,9 +232,9 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) a = btScalar(j)*(2*M__PI/m) + A[i0]; if (a > M__PI) a -= 2*M__PI; btScalar maxdiff=1e9,diff; -#if defined(DEBUG) || defined (_DEBUG) - *iret = i0; // iret is not allowed to keep this value -#endif + + *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); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index fad770ac26d..1db51a36d03 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -22,7 +22,7 @@ class btPoolAllocator; ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator size, default collision algorithms and persistent manifold pool size -///todo: describe the meaning +///@todo: describe the meaning class btCollisionConfiguration { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index c6728918d16..a6da5f61a3c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -17,7 +17,6 @@ subject to the following restrictions: #define COLLISION_CREATE_FUNC #include "LinearMath/btAlignedObjectArray.h" -typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; class btCollisionAlgorithm; class btCollisionObject; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index a031a9f9784..e6ff2130aad 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -52,12 +52,12 @@ btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisio for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) { m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); - assert(m_doubleDispatch[i][j]); + btAssert(m_doubleDispatch[i][j]); } } -}; +} void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) @@ -78,7 +78,13 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) btCollisionObject* body0 = (btCollisionObject*)b0; btCollisionObject* body1 = (btCollisionObject*)b1; - + + //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold' + //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold())); + btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()); + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); + void* mem = 0; if (m_persistentManifoldPoolAllocator->getFreeCount()) @@ -89,7 +95,7 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) mem = btAlignedAlloc(sizeof(btPersistentManifold),16); } - btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0); + btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); @@ -144,7 +150,6 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo - bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1) { //here you can do filtering @@ -158,8 +163,8 @@ bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionOb bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1) { - assert(body0); - assert(body1); + btAssert(body0); + btAssert(body1); bool needsCollision = true; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 846c9b9b989..285b8f174e4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: btCollisionObject::btCollisionObject() : m_anisotropicFriction(1.f,1.f,1.f), m_hasAnisotropicFriction(false), + m_contactProcessingThreshold(0.f), m_broadphaseHandle(0), m_collisionShape(0), m_rootCollisionShape(0), diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 8442868cf89..0d5b7886443 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -29,8 +29,11 @@ struct btBroadphaseProxy; class btCollisionShape; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btAlignedObjectArray.h" +typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; + /// 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. @@ -49,8 +52,10 @@ protected: //without destroying the continuous interpolated motion (which uses this interpolation velocities) btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; + btVector3 m_anisotropicFriction; - bool m_hasAnisotropicFriction; + bool m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; btBroadphaseProxy* m_broadphaseHandle; btCollisionShape* m_collisionShape; @@ -74,7 +79,7 @@ protected: ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer void* m_userObjectPointer; - ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody etc. + ///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; @@ -106,14 +111,19 @@ public: CF_STATIC_OBJECT= 1, CF_KINEMATIC_OBJECT= 2, CF_NO_CONTACT_RESPONSE = 4, - CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution) + CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + CF_CHARACTER_OBJECT = 16 }; enum CollisionObjectTypes { CO_COLLISION_OBJECT =1, CO_RIGID_BODY, - CO_SOFT_BODY + ///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, + CO_SOFT_BODY, + CO_HF_FLUID }; SIMD_FORCE_INLINE bool mergesSimulationIslands() const @@ -136,6 +146,16 @@ public: return m_hasAnisotropicFriction; } + ///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) + { + m_contactProcessingThreshold = contactProcessingThreshold; + } + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } SIMD_FORCE_INLINE bool isStaticObject() const { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; @@ -193,7 +213,7 @@ public: m_collisionShape = collisionShape; } - int getActivationState() const { return m_activationState1;} + SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} void setActivationState(int newState); @@ -210,7 +230,7 @@ public: void activate(bool forceActivation = false); - inline bool isActive() const + SIMD_FORCE_INLINE bool isActive() const { return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } @@ -254,12 +274,12 @@ public: } - btBroadphaseProxy* getBroadphaseHandle() + SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() { return m_broadphaseHandle; } - const btBroadphaseProxy* getBroadphaseHandle() const + SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const { return m_broadphaseHandle; } @@ -305,7 +325,7 @@ public: return m_interpolationAngularVelocity; } - const int getIslandTag() const + SIMD_FORCE_INLINE int getIslandTag() const { return m_islandTag1; } @@ -315,7 +335,7 @@ public: m_islandTag1 = tag; } - const int getCompanionId() const + SIMD_FORCE_INLINE int getCompanionId() const { return m_companionId; } @@ -325,7 +345,7 @@ public: m_companionId = id; } - const btScalar getHitFraction() const + SIMD_FORCE_INLINE btScalar getHitFraction() const { return m_hitFraction; } @@ -336,7 +356,7 @@ public: } - const int getCollisionFlags() const + SIMD_FORCE_INLINE int getCollisionFlags() const { return m_collisionFlags; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index d8674a320a7..10e880e2523 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -32,6 +32,9 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" +//#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 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -66,6 +69,7 @@ btCollisionWorld::~btCollisionWorld() // getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); getBroadphase()->destroyProxy(bp,m_dispatcher1); + collisionObject->setBroadphaseHandle(0); } } @@ -113,6 +117,41 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho } + + +void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) +{ + btVector3 minAabb,maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + //need to increase the aabb for contact thresholds + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; + + //moving objects should be moderately sized, probably something wrong if not + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + { + bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + } else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + colObj->setActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe && m_debugDrawer) + { + reportMe = false; + m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); + m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); + m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + m_debugDrawer->reportErrorWarning("Thanks.\n"); + } + } +} + void btCollisionWorld::updateAabbs() { BT_PROFILE("updateAabbs"); @@ -125,38 +164,9 @@ void btCollisionWorld::updateAabbs() //only update aabb of active objects if (colObj->isActive()) { - btPoint3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - //need to increase the aabb for contact thresholds - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); - minAabb -= contactThreshold; - maxAabb += contactThreshold; - - btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; - - //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) - { - bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } else - { - //something went wrong, investigate - //this assert is unwanted in 3D modelers (danger of loosing work) - colObj->setActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe && m_debugDrawer) - { - reportMe = false; - m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); - m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); - m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); - m_debugDrawer->reportErrorWarning("Thanks.\n"); - } - } + updateSingleAabb(colObj); } } - } @@ -226,6 +236,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra if (collisionShape->isConvex()) { +// BT_PROFILE("rayTestConvex"); btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; @@ -269,6 +280,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } else { if (collisionShape->isConcave()) { +// BT_PROFILE("rayTestConcave"); if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape @@ -286,7 +298,8 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - btTriangleRaycastCallback(from,to), + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) @@ -317,7 +330,8 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); } else { - btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; + //generic (slower) case + btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); @@ -330,11 +344,12 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra { btCollisionWorld::RayResultCallback* m_resultCallback; btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + btConcaveShape* m_triangleMesh; BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - btTriangleRaycastCallback(from,to), + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh): + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) @@ -363,7 +378,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra }; - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; @@ -371,10 +386,11 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); - triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } } else { - //todo: use AABB tree or other BVH acceleration structure! +// BT_PROFILE("rayTestCompound"); + ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt if (collisionShape->isCompound()) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); @@ -408,9 +424,10 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { if (collisionShape->isConvex()) { + //BT_PROFILE("convexSweepConvex"); btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; - castResult.m_fraction = btScalar(1.);//?? + castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; @@ -452,6 +469,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { + //BT_PROFILE("convexSweepbtBvhTriangleMesh"); btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); @@ -508,7 +526,8 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); } else { - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + //BT_PROFILE("convexSweepConcave"); + btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); @@ -520,10 +539,10 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { btCollisionWorld::ConvexResultCallback* m_resultCallback; btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + btConcaveShape* m_triangleMesh; BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): + btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), m_resultCallback(resultCallback), m_collisionObject(collisionObject), @@ -556,7 +575,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); @@ -567,12 +586,13 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt rayAabbMaxLocal.setMax(convexToLocal); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; - triangleMesh->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); } } else { - //todo: use AABB tree or other BVH acceleration structure! + ///@todo : use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) { + BT_PROFILE("convexSweepCompound"); const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); int i=0; for (i=0;i<compoundShape->getNumChildShapes();i++) @@ -596,51 +616,173 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt } } -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const + +struct btSingleRayCallback : public btBroadphaseRayCallback { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; - btTransform rayFromTrans,rayToTrans; - rayFromTrans.setIdentity(); - rayFromTrans.setOrigin(rayFromWorld); - rayToTrans.setIdentity(); + const btCollisionWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; - rayToTrans.setOrigin(rayToWorld); + 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); - /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) + btVector3 rayDir = (rayToWorld-rayFromWorld); - int i; - for (i=0;i<m_collisionObjects.size();i++) + rayDir.normalize (); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + 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); + + } + + + + virtual bool process(const btBroadphaseProxy* proxy) { ///terminate further ray tests, once the closestHitFraction reached zero - if (resultCallback.m_closestHitFraction == btScalar(0.f)) - break; + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - btCollisionObject* collisionObject= m_collisionObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; +#if 0 +#ifdef RECALCULATE_AABB btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - - btScalar hitLambda = resultCallback.m_closestHitFraction; - btVector3 hitNormal; - if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) +#else + //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); + const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; + const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; +#endif +#endif + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) { - rayTestSingle(rayFromTrans,rayToTrans, + m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, collisionObject, collisionObject->getCollisionShape(), collisionObject->getWorldTransform(), - resultCallback); + m_resultCallback); } } - + return true; } +}; + +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); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); +#else + for (int i=0;i<this->getNumCollisionObjects();i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } -void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback) const + +struct btSingleSweepCallback : public btBroadphaseRayCallback { + + 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) + { + btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); + btVector3 rayDir = unnormalizedRayDir.normalized(); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(unnormalizedRayDir); + + } + + 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; + + //only perform raycast if filterMask matches + 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); + } + + return true; + } +}; + + + +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; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; @@ -649,12 +791,21 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT { btVector3 linVel, angVel; btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btVector3 zeroLinVel; + zeroLinVel.setValue(0,0,0); btTransform R; R.setIdentity (); R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); } +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + + btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); + + 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; @@ -676,9 +827,9 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT collisionObject->getCollisionShape(), collisionObject->getWorldTransform(), resultCallback, - getDispatchInfo().m_allowedCcdPenetration); + allowedCcdPenetration); } } } - +#endif //USE_BRUTEFORCE_RAYBROADPHASE } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 7557691a9a9..87f7137a55b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans http://bulletphysics.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. @@ -22,39 +22,39 @@ subject to the following restrictions: * * 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 ). * - * There is the Physics Forum for Feedback and bteral Collision Detection and Physics discussions. - * Please visit http://www.continuousphysics.com/Bullet/phpBB2/index.php + * There is the Physics Forum for feedback and general Collision Detection and Physics discussions. + * Please visit http://www.bulletphysics.com * * @section install_sec Installation * * @subsection step1 Step 1: Download - * You can download the Bullet Physics Library from our website: http://www.continuousphysics.com/Bullet/ + * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list * @subsection step2 Step 2: Building * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8. * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version). * - * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using cmake, http://www.cmake.org, or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet. + * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet. + * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam. * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files. - * So if you are not using MSVC, you can run configure and jam . - * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/pub/jam/ + * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam * * @subsection step3 Step 3: Testing demos - * Try to run and experiment with CcdPhysicsDemo executable as a starting point. + * Try to run and experiment with BasicDemo executable as a starting point. * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation. * The Dependencies can be seen in this documentation under Directories * - * @subsection step4 Step 4: Integrating in your application, Full Rigid Body Simulation - * Check out CcdPhysicsDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. - * PLEASE NOTE THE CcdPhysicsEnvironment and CcdPhysicsController is obsolete and will be removed. It has been replaced by classes derived frmo btDynamicsWorld and btRididBody + * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation + * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. + * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld. * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras) * Bullet Collision Detection can also be used without the Dynamics/Extras. - * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. Also in Extras/test_BulletOde.cpp there is a sample Collision Detection integration with Open Dynamics Engine, ODE, http://www.ode.org + * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation. * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. * * @section copyright Copyright - * Copyright (C) 2005-2007 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon - * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, + * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon + * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt. * */ @@ -71,7 +71,7 @@ class btBroadphaseInterface; #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "btCollisionObject.h" -#include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray +#include "btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" #include "LinearMath/btAlignedObjectArray.h" @@ -107,6 +107,11 @@ public: m_broadphasePairCache = pairCache; } + const btBroadphaseInterface* getBroadphase() const + { + return m_broadphasePairCache; + } + btBroadphaseInterface* getBroadphase() { return m_broadphasePairCache; @@ -128,8 +133,10 @@ public: return m_dispatcher1; } - virtual void updateAabbs(); + void updateSingleAabb(btCollisionObject* colObj); + virtual void updateAabbs(); + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) { m_debugDrawer = debugDrawer; @@ -179,6 +186,8 @@ public: btCollisionObject* m_collisionObject; short int m_collisionFilterGroup; short int m_collisionFilterMask; + //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback + unsigned int m_flags; virtual ~RayResultCallback() { @@ -192,7 +201,9 @@ public: :m_closestHitFraction(btScalar(1.)), m_collisionObject(0), m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + //@BP Mod + m_flags(0) { } @@ -347,7 +358,7 @@ public: // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. - void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback) const; + void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 535b61992b0..1dea91a0b0b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -19,19 +19,32 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btAabbUtil2.h" +#include "btManifoldResult.h" btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) -:btCollisionAlgorithm(ci), +:btActivatingCollisionAlgorithm(ci,body0,body1), m_isSwapped(isSwapped), m_sharedManifold(ci.m_manifold) { m_ownsManifold = false; btCollisionObject* colObj = m_isSwapped? body1 : body0; + btAssert (colObj->getCollisionShape()->isCompound()); + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); + m_compoundShapeRevision = compoundShape->getUpdateRevision(); + + preallocateChildAlgorithms(body0,body1); +} + +void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1) +{ + btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->getCollisionShape()->isCompound()); + btAssert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); + int numChildren = compoundShape->getNumChildShapes(); int i; @@ -46,14 +59,13 @@ m_sharedManifold(ci.m_manifold) btCollisionShape* tmpShape = colObj->getCollisionShape(); btCollisionShape* childShape = compoundShape->getChildShape(i); colObj->internalSetTemporaryCollisionShape( childShape ); - m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold); + m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold); colObj->internalSetTemporaryCollisionShape( tmpShape ); } } } - -btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() +void btCompoundCollisionAlgorithm::removeChildAlgorithms() { int numChildren = m_childCollisionAlgorithms.size(); int i; @@ -67,6 +79,11 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() } } +btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() +{ + removeChildAlgorithms(); +} + @@ -167,13 +184,50 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->getCollisionShape()->isCompound()); + + + btAssert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); + ///btCompoundShape might have changed: + ////make sure the internal child collision algorithm caches are still valid + if (compoundShape->getUpdateRevision() != m_compoundShapeRevision) + { + ///clear and update all + removeChildAlgorithms(); + + preallocateChildAlgorithms(body0,body1); + } + + btDbvt* tree = compoundShape->getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps btCompoundLeafCallback callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); + ///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++) + { + if (m_childCollisionAlgorithms[i]) + { + m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); + for (int m=0;m<manifoldArray.size();m++) + { + if (manifoldArray[m]->getNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0);//??necessary? + } + } + manifoldArray.clear(); + } + } + } if (tree) { @@ -242,7 +296,7 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->getCollisionShape()->isCompound()); + btAssert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); @@ -285,3 +339,4 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* } + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 624a3cf10f5..255e0af668c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef COMPOUND_COLLISION_ALGORITHM_H #define COMPOUND_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" @@ -26,16 +26,23 @@ class btDispatcher; #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" class btDispatcher; +class btCollisionObject; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -class btCompoundCollisionAlgorithm : public btCollisionAlgorithm +class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; class btPersistentManifold* m_sharedManifold; bool m_ownsManifold; + + int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated + + void removeChildAlgorithms(); + void preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1); + public: btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 6d28904cb03..cbc5530732b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,body0,body1), m_isSwapped(isSwapped), m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) { @@ -72,7 +72,7 @@ btConvexTriangleCallback::~btConvexTriangleCallback() void btConvexTriangleCallback::clearCache() { m_dispatcher->clearManifold(m_manifoldPtr); -}; +} @@ -93,7 +93,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { btVector3 color(255,255,0); btTransform& tr = ob->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 34b9a107be9..984a4c39e8e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H #define CONVEX_CONCAVE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionShapes/btTriangleCallback.h" @@ -34,8 +34,8 @@ class btConvexTriangleCallback : public btTriangleCallback btVector3 m_aabbMin; btVector3 m_aabbMax ; - btManifoldResult* m_resultOut; + btManifoldResult* m_resultOut; btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; @@ -70,7 +70,7 @@ int m_triangleCount; /// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -class btConvexConcaveCollisionAlgorithm : public btCollisionAlgorithm +class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_isSwapped; @@ -78,6 +78,7 @@ class btConvexConcaveCollisionAlgorithm : public btCollisionAlgorithm btConvexTriangleCallback m_btConvexTriangleCallback; + public: btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index cb830f889be..274c5f5bdc6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -38,7 +38,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" @@ -51,6 +51,8 @@ subject to the following restrictions: btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) { + m_numPerturbationIterations = 0; + m_minimumPointsPerturbationThreshold = 3; m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } @@ -59,17 +61,22 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc() { } -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) -: btCollisionAlgorithm(ci), -m_gjkPairDetector(0,0,simplexSolver,pdSolver), +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) +: btActivatingCollisionAlgorithm(ci,body0,body1), +m_simplexSolver(simplexSolver), +m_pdSolver(pdSolver), m_ownManifold (false), m_manifoldPtr(mf), -m_lowLevelOfDetail(false) +m_lowLevelOfDetail(false), +#ifdef USE_SEPDISTANCE_UTIL2 +,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), + (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), +#endif +m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { (void)body0; (void)body1; - - } @@ -90,8 +97,63 @@ void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool 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_perturbA(perturbA), + m_unPerturbedTransform(unPerturbedTransform), + m_debugDrawer(debugDrawer) + { + } + virtual ~ btPerturbedContactResult() + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth) + { + 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 + { + 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_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth); + } +}; +extern btScalar gContactBreakingThreshold; // // Convex-Convex collision algorithm @@ -107,39 +169,125 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } resultOut->setPersistentManifold(m_manifoldPtr); -#ifdef USE_BT_GJKEPA - btConvexShape* shape0(static_cast<btConvexShape*>(body0->getCollisionShape())); - btConvexShape* shape1(static_cast<btConvexShape*>(body1->getCollisionShape())); - const btScalar radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/); - btGjkEpaSolver::sResults results; - if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(), - shape1,body1->getWorldTransform(), - radialmargin,results)) - { - dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - } -#else + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); + btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); + +#ifdef USE_SEPDISTANCE_UTIL2 + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) +#endif //USE_SEPDISTANCE_UTIL2 + + { + btGjkPairDetector::ClosestPointInput input; + btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) - m_gjkPairDetector.setMinkowskiA(min0); - m_gjkPairDetector.setMinkowskiB(min1); - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; - input.m_stackAlloc = dispatchInfo.m_stackAllocator; + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); -// input.m_maximumDistanceSquared = btScalar(1e30); - +#ifdef USE_SEPDISTANCE_UTIL2 + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + input.m_maximumDistanceSquared = 1e30f; + } else +#endif //USE_SEPDISTANCE_UTIL2 + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + } + + input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold; + + //now perturbe directions to get multiple contact points + btVector3 v0,v1; + btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points + if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) + { + + int i; + + 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 + { + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; + if (perturbeA) + { + unPerturbedTransform = input.m_transformA; + } else + { + unPerturbedTransform = input.m_transformB; + } + + for ( i=0;i<m_numPerturbationIterations;i++) + { + 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)*body0->getWorldTransform().getBasis()); + input.m_transformB = body1->getWorldTransform(); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); +#endif //DEBUG_CONTACTS + } else + { + input.m_transformA = body0->getWorldTransform(); + input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->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) + { + m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); + } +#endif //USE_SEPDISTANCE_UTIL2 + + + } if (m_ownManifold) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index a0030e6793d..62dd33eb98d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -16,30 +16,50 @@ subject to the following restrictions: #ifndef CONVEX_CONVEX_ALGORITHM_H #define CONVEX_CONVEX_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil class btConvexPenetrationDepthSolver; -///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations. -class btConvexConvexAlgorithm : public btCollisionAlgorithm +///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise +///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions. +///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util +///for certain pairs that have a small size ratio +///#define USE_SEPDISTANCE_UTIL2 1 + +///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects. +///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal. +///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888 +class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { - btGjkPairDetector m_gjkPairDetector; -public: +#ifdef USE_SEPDISTANCE_UTIL2 + btConvexSeparatingDistanceUtil m_sepDistance; +#endif + btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + 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,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + virtual ~btConvexConvexAlgorithm(); @@ -65,9 +85,12 @@ public: struct CreateFunc :public btCollisionAlgorithmCreateFunc { + btConvexPenetrationDepthSolver* m_pdSolver; btSimplexSolverInterface* m_simplexSolver; - + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); virtual ~CreateFunc(); @@ -75,7 +98,7 @@ public: virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); - return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver); + return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index 391cf6c7bc4..a7b3b163d66 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -22,15 +22,17 @@ subject to the following restrictions: //#include <stdio.h> -btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) : btCollisionAlgorithm(ci), m_ownManifold(false), m_manifoldPtr(mf), -m_isSwapped(isSwapped) +m_isSwapped(isSwapped), +m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { btCollisionObject* convexObj = m_isSwapped? col1 : col0; btCollisionObject* planeObj = m_isSwapped? col0 : col1; - + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) { m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); @@ -48,30 +50,28 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() } } - - -void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - (void)dispatchInfo; - (void)resultOut; - if (!m_manifoldPtr) - return; - - btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* planeObj = m_isSwapped? body0: body1; btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - bool hasCollision = false; + bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - btTransform planeInConvex; - planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + + btTransform convexWorldTransform = convexObj->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); - + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform; + //now perturbe the convex-world transform + convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); + btTransform planeInConvex; + planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform(); + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); @@ -87,6 +87,53 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0 btVector3 pOnB = vtxInPlaneWorld; resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); } +} + + +void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + if (!m_manifoldPtr) + return; + + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + + //first perform a collision query with the non-perturbated collision objects + { + btQuaternion rotq(0,0,0,1); + collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); + } + + if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) + { + 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; + + 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,body0,body1,dispatchInfo,resultOut); + } + } + if (m_ownManifold) { if (m_manifoldPtr->getNumContacts()) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index 7b258554171..368ca71dda0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -28,18 +28,22 @@ class btPersistentManifold; /// Other features are frame-coherency (persistent data) and collision response. class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm { - bool m_ownManifold; + bool m_ownManifold; btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - + bool m_isSwapped; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + public: - btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); virtual ~btConvexPlaneCollisionAlgorithm(); virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void collideSingleContact (const btQuaternion& perturbeRot, 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) @@ -52,15 +56,24 @@ public: struct CreateFunc :public btCollisionAlgorithmCreateFunc { + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc() + : m_numPerturbationIterations(3), + m_minimumPointsPerturbationThreshold(3) + { + } + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false); + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } else { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true); + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 1c317080544..3ae25f109c2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -100,11 +100,10 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); int maxSize3 = sizeof(btCompoundCollisionAlgorithm); - int maxSize4 = sizeof(btEmptyAlgorithm); - + int sl = sizeof(btConvexSeparatingDistanceUtil); + sl = sizeof(btGjkPairDetector); int collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); if (constructionInfo.m_stackAlloc) { @@ -289,3 +288,10 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg //failed to find an algorithm return m_emptyCreateFunc; } + +void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +{ + btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc; + convexConvex->m_numPerturbationIterations = numPerturbationIterations; + convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index df48ee95a18..6d8cab726bd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -33,9 +33,9 @@ struct btDefaultCollisionConstructionInfo :m_stackAlloc(0), m_persistentManifoldPool(0), m_collisionAlgorithmPool(0), - m_defaultMaxPersistentManifoldPoolSize(65535), - m_defaultMaxCollisionAlgorithmPoolSize(65535), - m_defaultStackAllocatorSize(5*1024*1024) + m_defaultMaxPersistentManifoldPoolSize(4096), + m_defaultMaxCollisionAlgorithmPoolSize(4096), + m_defaultStackAllocatorSize(0) { } }; @@ -44,7 +44,7 @@ struct btDefaultCollisionConstructionInfo ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator, pool memory allocators -///todo: describe the meaning +///@todo: describe the meaning class btDefaultCollisionConfiguration : public btCollisionConfiguration { @@ -111,6 +111,14 @@ public: virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(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. + ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature. + ///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled + ///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); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp new file mode 100644 index 00000000000..86141fa6899 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -0,0 +1,171 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btGhostObject.h" +#include "btCollisionWorld.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "LinearMath/btAabbUtil2.h" + +btGhostObject::btGhostObject() +{ + m_internalType = CO_GHOST_OBJECT; +} + +btGhostObject::~btGhostObject() +{ + ///btGhostObject should have been removed from the world, so no overlapping objects + btAssert(!m_overlappingObjects.size()); +} + + +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()) + { + //not found + m_overlappingObjects.push_back(otherObject); + } +} + +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()) + { + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects.pop_back(); + } +} + + +btPairCachingGhostObject::btPairCachingGhostObject() +{ + m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); +} + +btPairCachingGhostObject::~btPairCachingGhostObject() +{ + m_hashPairCache->~btHashedOverlappingPairCache(); + btAlignedFree( m_hashPairCache ); +} + +void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + 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()) + { + m_overlappingObjects.push_back(otherObject); + m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + } +} + +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index<m_overlappingObjects.size()) + { + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects.pop_back(); + m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher); + } +} + + +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans = convexFromWorld; + convexToTrans = convexToWorld; + btVector3 castShapeAabbMin, castShapeAabbMax; + /* Compute AABB that encompasses angular movement */ + { + btVector3 linVel, angVel; + btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransform R; + R.setIdentity (); + R.setRotation (convexFromTrans.getRotation()); + castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + } + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;i<m_overlappingObjects.size();i++) + { + btCollisionObject* collisionObject= m_overlappingObjects[i]; + //only perform raycast if filterMask matches + if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + 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 +{ + btTransform rayFromTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + btTransform rayToTrans; + rayToTrans.setIdentity(); + rayToTrans.setOrigin(rayToWorld); + + + int i; + for (i=0;i<m_overlappingObjects.size();i++) + { + btCollisionObject* collisionObject= m_overlappingObjects[i]; + //only perform raycast if filterMask matches + if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + 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 new file mode 100644 index 00000000000..95b5750240c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -0,0 +1,174 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_GHOST_OBJECT_H +#define BT_GHOST_OBJECT_H + + +#include "btCollisionObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" +#include "LinearMath/btAlignedAllocator.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "btCollisionWorld.h" + +class btConvexShape; + +class btDispatcher; + +///The btGhostObject can keep track of all objects that are overlapping +///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 +{ +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 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); + ///this method is mainly for expert/internal use only. + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + int getNumOverlappingObjects() const + { + return m_overlappingObjects.size(); + } + + btCollisionObject* getOverlappingObject(int index) + { + return m_overlappingObjects[index]; + } + + const btCollisionObject* getOverlappingObject(int index) const + { + return m_overlappingObjects[index]; + } + + btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs() + { + return m_overlappingObjects; + } + + const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const + { + return m_overlappingObjects; + } + + // + // internal cast + // + + static const btGhostObject* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (const btGhostObject*)colObj; + return 0; + } + static btGhostObject* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (btGhostObject*)colObj; + return 0; + } + +}; + +class btPairCachingGhostObject : public btGhostObject +{ + 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 removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + 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() + { + } + + virtual ~btGhostPairCallback() + { + + } + + 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); + if (ghost0) + ghost0->addOverlappingObjectInternal(proxy1, proxy0); + if (ghost1) + ghost1->addOverlappingObjectInternal(proxy0, proxy1); + return 0; + } + + 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); + if (ghost0) + ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); + if (ghost1) + ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); + return 0; + } + + 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
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index d5b12598be2..f8dfa5b101f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -55,7 +55,7 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { - assert(m_manifoldPtr); + btAssert(m_manifoldPtr); //order in manifold needs to match if (depth > m_manifoldPtr->getContactBreakingThreshold()) @@ -92,8 +92,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; - - ///todo, check this for any side effects + //printf("depth=%f\n",depth); + ///@todo, check this for any side effects if (insertIndex >= 0) { //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 5aac9a46f6a..964b6a04483 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -45,6 +45,8 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result int m_partId1; int m_index0; int m_index1; + + public: btManifoldResult() @@ -77,6 +79,7 @@ public: m_index1=index1; } + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); SIMD_FORCE_INLINE void refreshContactPoints() diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 9f8e27a4407..0328d0f738f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -24,7 +24,8 @@ subject to the following restrictions: //#include <stdio.h> #include "LinearMath/btQuickprof.h" -btSimulationIslandManager::btSimulationIslandManager() +btSimulationIslandManager::btSimulationIslandManager(): +m_splitIslands(true) { } @@ -43,10 +44,10 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl { { - btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); - + for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++) { + btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); const btBroadphasePair& collisionPair = pairPtr[i]; btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; @@ -143,11 +144,13 @@ class btPersistentManifoldSortPredicate }; -void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects) +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 @@ -183,7 +186,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio // printf("error in island management\n"); } - assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if (colObj0->getActivationState()== ACTIVE_TAG) @@ -210,7 +213,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio // printf("error in island management\n"); } - assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { @@ -231,13 +234,14 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio // printf("error in island management\n"); } - assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if ( colObj0->getActivationState() == ISLAND_SLEEPING) { colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setDeactivationTime(0.f); } } } @@ -248,11 +252,11 @@ 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++) @@ -262,7 +266,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0()); btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1()); - //todo: check sleeping conditions! + ///@todo: check sleeping conditions! if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) { @@ -276,24 +280,24 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio { colObj0->activate(); } -#ifdef SPLIT_ISLANDS - // //filtering for response - if (dispatcher->needsResponse(colObj0,colObj1)) - m_islandmanifold.push_back(manifold); -#endif //SPLIT_ISLANDS + if(m_splitIslands) + { + //filtering for response + if (dispatcher->needsResponse(colObj0,colObj1)) + m_islandmanifold.push_back(manifold); + } } } } -// -// todo: this is random access, it can be walked 'cache friendly'! -// -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) +///@todo: this is random access, it can be walked 'cache friendly'! +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) { + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - buildIslands(dispatcher,collisionObjects); + buildIslands(dispatcher,collisionWorld); int endIslandIndex=1; int startIslandIndex; @@ -301,84 +305,86 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, BT_PROFILE("processIslands"); -#ifndef SPLIT_ISLANDS - btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); - - callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); -#else - // Sort manifolds, based on islands - // Sort the vector using predicate and std::sort - //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + if(!m_splitIslands) + { + btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + } + else + { + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); - int numManifolds = int (m_islandmanifold.size()); + int numManifolds = int (m_islandmanifold.size()); - //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); - //now process all active islands (sets of manifolds for now) + //now process all active islands (sets of manifolds for now) - int startManifoldIndex = 0; - int endManifoldIndex = 1; + int startManifoldIndex = 0; + int endManifoldIndex = 1; - //int islandId; + //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) - { - int islandId = getUnionFind().getElement(startIslandIndex).m_id; + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; - bool islandSleeping = false; - - for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) - { - int i = getUnionFind().getElement(endIslandIndex).m_sz; - btCollisionObject* colObj0 = collisionObjects[i]; - m_islandBodies.push_back(colObj0); - if (!colObj0->isActive()) - islandSleeping = true; - } - + bool islandSleeping = false; + + for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) + { + int i = getUnionFind().getElement(endIslandIndex).m_sz; + btCollisionObject* colObj0 = collisionObjects[i]; + m_islandBodies.push_back(colObj0); + if (!colObj0->isActive()) + islandSleeping = true; + } + - //find the accompanying contact manifold for this islandId - int numIslandManifolds = 0; - btPersistentManifold** startManifold = 0; + //find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + btPersistentManifold** startManifold = 0; - if (startManifoldIndex<numManifolds) - { - int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]); - if (curIslandId == islandId) + if (startManifoldIndex<numManifolds) { - startManifold = &m_islandmanifold[startManifoldIndex]; - - for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) + 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++) + { + } + /// Process the actual simulation, only if not sleeping/deactivated + numIslandManifolds = endManifoldIndex-startManifoldIndex; } - /// Process the actual simulation, only if not sleeping/deactivated - numIslandManifolds = endManifoldIndex-startManifoldIndex; + } - } + if (!islandSleeping) + { + callback->ProcessIsland(&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; + } - 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); - } - - if (numIslandManifolds) - { - startManifoldIndex = endManifoldIndex; + m_islandBodies.resize(0); } - - m_islandBodies.resize(0); - } -#endif //SPLIT_ISLANDS - + } // else if(!splitIslands) } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 5f4d54cd803..d059f5d6b0d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -19,7 +19,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" - +#include "btCollisionObject.h" class btCollisionObject; class btCollisionWorld; @@ -35,6 +35,7 @@ class btSimulationIslandManager btAlignedObjectArray<btPersistentManifold*> m_islandmanifold; btAlignedObjectArray<btCollisionObject* > m_islandBodies; + bool m_splitIslands; public: btSimulationIslandManager(); @@ -61,9 +62,18 @@ public: virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; - void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); + void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); + + void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); - void buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects); + bool getSplitIslands() + { + return m_splitIslands; + } + void setSplitIslands(bool doSplitIslands) + { + m_splitIslands = doSplitIslands; + } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 1e4bbce451d..8df876928c1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: //#include <stdio.h> btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,col0,col1), m_ownManifold(false), m_manifoldPtr(mf), m_isSwapped(isSwapped) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 158f0ffcc74..47111d1c4af 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SPHERE_BOX_COLLISION_ALGORITHM_H #define SPHERE_BOX_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; @@ -26,7 +26,7 @@ class btPersistentManifold; /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. /// Other features are frame-coherency (persistent data) and collision response. -class btSphereBoxCollisionAlgorithm : public btCollisionAlgorithm +class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index c2b13f5903f..5c4e78fe518 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,col0,col1), m_ownManifold(false), m_manifoldPtr(mf) { @@ -78,7 +78,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 } ///point on A (worldspace) - btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; + ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index afdc2ee7fa0..7d07512ca66 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H #define SPHERE_SPHERE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" @@ -26,7 +26,7 @@ class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// Other features are frame-coherency (persistent data) and collision response. /// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm +class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; @@ -35,7 +35,7 @@ public: btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} + : btActivatingCollisionAlgorithm(ci) {} virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index fdbd4abef27..2d5efcf56ba 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,col0,col1), m_ownManifold(false), m_manifoldPtr(mf), m_swapped(swapped) @@ -56,10 +56,10 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle); + SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds input.m_transformA = sphereObj->getWorldTransform(); input.m_transformB = triObj->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index bbaf228c44c..606c3635ae9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; @@ -25,7 +25,7 @@ class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// Other features are frame-coherency (persistent data) and collision response. /// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereTriangleCollisionAlgorithm : public btCollisionAlgorithm +class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; @@ -35,7 +35,7 @@ public: btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped); btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} + : btActivatingCollisionAlgorithm(ci) {} virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index bb01b60fa85..c561df06109 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -14,8 +14,6 @@ subject to the following restrictions: */ #include "btUnionFind.h" -#include <assert.h> - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index 820c8bc858e..e105ecbff18 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -98,8 +98,8 @@ class btUnionFind int find(int x) { - //assert(x < m_N); - //assert(x >= 0); + //btAssert(x < m_N); + //btAssert(x >= 0); while (x != m_elements[x].m_id) { @@ -110,8 +110,8 @@ class btUnionFind m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; #endif // x = m_elements[x].m_id; - //assert(x < m_N); - //assert(x >= 0); + //btAssert(x < m_N); + //btAssert(x >= 0); } return x; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index 4d4fbefea37..14502d485dc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -19,7 +19,7 @@ subject to the following restrictions: #include "btPolyhedralConvexShape.h" #include "btCollisionMargin.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "LinearMath/btPoint3.h" +#include "LinearMath/btVector3.h" #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. @@ -45,8 +45,6 @@ public: } - virtual int getShapeType() const { return BOX_SHAPE_PROXYTYPE;} - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); @@ -82,8 +80,10 @@ public: } - btBoxShape( const btVector3& boxHalfExtents) + btBoxShape( const btVector3& boxHalfExtents) + : btPolyhedralConvexShape() { + m_shapeType = BOX_SHAPE_PROXYTYPE; btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; }; @@ -117,7 +117,7 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const { //this plane might not be aligned... btVector4 plane ; @@ -161,36 +161,30 @@ public: switch (i) { case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - plane[3] = -halfExtents.x(); + plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); break; case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - plane[3] = -halfExtents.x(); + plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); break; case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - plane[3] = -halfExtents.y(); + plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); break; case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - plane[3] = -halfExtents.y(); + plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); break; case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - plane[3] = -halfExtents.z(); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); break; case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - plane[3] = -halfExtents.z(); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); break; default: - assert(0); + btAssert(0); } } - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -261,7 +255,7 @@ public: - virtual bool isInside(const btPoint3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt,btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); @@ -312,7 +306,7 @@ public: penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); break; default: - assert(0); + btAssert(0); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 819f6f7f97d..68ff5e2d7bb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -26,6 +26,7 @@ m_bvh(0), m_useQuantizedAabbCompression(useQuantizedAabbCompression), m_ownsBvh(false) { + m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface #ifndef DISABLE_BVH @@ -57,6 +58,7 @@ m_bvh(0), m_useQuantizedAabbCompression(useQuantizedAabbCompression), m_ownsBvh(false) { + m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface #ifndef DISABLE_BVH @@ -141,10 +143,19 @@ void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const for (int j=2;j>=0;j--) { int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - - btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + + 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()); + } + 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()); + } } /* Perform ray vs. triangle collision here */ @@ -202,9 +213,18 @@ void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, co { int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + 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()); + } + 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()); + } } /* Perform ray vs. triangle collision here */ @@ -279,12 +299,24 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co #ifdef DEBUG_TRIANGLE_MESH printf("%d ,",graphicsindex); #endif //DEBUG_TRIANGLE_MESH - btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + 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()); + } + 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()); + } #ifdef DEBUG_TRIANGLE_MESH printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z()); #endif //DEBUG_TRIANGLE_MESH diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 6e587340dc1..cb2c326574e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -37,7 +37,7 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btBvhTriangleMeshShape() :btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {}; + btBvhTriangleMeshShape() : btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;}; 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 @@ -50,10 +50,7 @@ public: return m_ownsBvh; } - virtual int getShapeType() const - { - return TRIANGLE_MESH_SHAPE_PROXYTYPE; - } + void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget); void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 26ba276b900..60a96a542f9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -19,8 +19,9 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) +btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () { + m_shapeType = CAPSULE_SHAPE_PROXYTYPE; m_upAxis = 1; m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h index d4b046d40c9..828c1b3a565 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -30,7 +30,7 @@ protected: protected: ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. - btCapsuleShape() {}; + btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; public: btCapsuleShape(btScalar radius,btScalar height); @@ -43,15 +43,13 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getShapeType() const { return CAPSULE_SHAPE_PROXYTYPE; } - virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 halfExtents(getRadius(),getRadius(),getRadius()); halfExtents[m_upAxis] = getRadius() + getHalfHeight(); halfExtents += btVector3(getMargin(),getMargin(),getMargin()); btMatrix3x3 abs_b = t.getBasis().absolute(); - btPoint3 center = t.getOrigin(); + btVector3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); aabbMin = center - extent; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 39e621aa946..d242cba1b72 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -16,6 +16,9 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionShape.h" +btScalar gContactThresholdFactor=btScalar(0.02); + + /* Make sure this dummy function never changes so that it can be used by probes that are checking whether the @@ -42,8 +45,13 @@ void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) con center = (aabbMin+aabbMax)*btScalar(0.5); } +btScalar btCollisionShape::getContactBreakingThreshold() const +{ + return getAngularMotionDisc() * gContactThresholdFactor; +} btScalar btCollisionShape::getAngularMotionDisc() const { + ///@todo cache this value, to improve performance btVector3 center; btScalar disc; getBoundingSphere(center,disc); @@ -65,7 +73,7 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b // add linear motion btVector3 linMotion = linvel*timeStep; - //todo: simd would have a vector max/min operation, instead of per-element access + ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.x() > btScalar(0.)) temporalAabbMaxx += linMotion.x(); else diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index 3a352b90777..1f4b9bec647 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -19,20 +19,21 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btPoint3.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. class btCollisionShape { - +protected: + int m_shapeType; void* m_userPointer; public: - btCollisionShape() : m_userPointer(0) + btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0) { } + virtual ~btCollisionShape() { } @@ -45,6 +46,8 @@ public: ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. virtual btScalar getAngularMotionDisc() const; + virtual btScalar getContactBreakingThreshold() const; + ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative @@ -76,7 +79,7 @@ public: return btBroadphaseProxy::isInfinite(getShapeType()); } - virtual int getShapeType() const=0; + virtual void setLocalScaling(const btVector3& scaling) =0; virtual const btVector3& getLocalScaling() const =0; virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0; @@ -87,13 +90,13 @@ public: #endif //__SPU__ - + int getShapeType() const { return m_shapeType; } 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; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 997361a5069..9c2b04d18fa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -17,16 +17,22 @@ subject to the following restrictions: #include "btCollisionShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" -btCompoundShape::btCompoundShape() -:m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), +btCompoundShape::btCompoundShape(bool enableDynamicAabbTree) +: m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)), m_collisionMargin(btScalar(0.)), m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), -m_dynamicAabbTree(0) +m_dynamicAabbTree(0), +m_updateRevision(1) { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + m_shapeType = COMPOUND_SHAPE_PROXYTYPE; + + if (enableDynamicAabbTree) + { + void* mem = btAlignedAlloc(sizeof(btDbvt),16); + m_dynamicAabbTree = new(mem) btDbvt(); + btAssert(mem==m_dynamicAabbTree); + } } @@ -41,6 +47,7 @@ btCompoundShape::~btCompoundShape() void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) { + m_updateRevision++; //m_childTransforms.push_back(localTransform); //m_childShapes.push_back(shape); btCompoundShapeChild child; @@ -49,6 +56,7 @@ 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); @@ -72,10 +80,29 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio } m_children.push_back(child); + +} + +void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform) +{ + 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); + int index = m_children.size()-1; + m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); + } + + recalculateLocalAabb(); } void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) { + m_updateRevision++; btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); if (m_dynamicAabbTree) { @@ -86,8 +113,11 @@ void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) } + + 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--) @@ -97,6 +127,8 @@ void btCompoundShape::removeChildShape(btCollisionShape* shape) m_children.swap(i,m_children.size()-1); m_children.pop_back(); //remove it from the m_dynamicAabbTree too + //@todo: this leads to problems due to caching in the btCompoundCollisionAlgorithm + //so effectively, removeChildShape is broken at the moment //m_dynamicAabbTree->remove(m_aabbProxies[i]); //m_aabbProxies.swap(i,m_children.size()-1); //m_aabbProxies.pop_back(); @@ -112,6 +144,7 @@ void btCompoundShape::recalculateLocalAabb() { // Recalculate the local aabb // Brute force, it iterates over all the shapes left. + m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); @@ -134,19 +167,27 @@ void btCompoundShape::recalculateLocalAabb() void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); + + //avoid an illegal AABB when there are no children + if (!m_children.size()) + { + localHalfExtents.setValue(0,0,0); + localCenter.setValue(0,0,0); + } + localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); + btMatrix3x3 abs_b = trans.getBasis().absolute(); - btPoint3 center = trans(localCenter); + btVector3 center = trans(localCenter); btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), abs_b[1].dot(localHalfExtents), abs_b[2].dot(localHalfExtents)); aabbMin = center-extent; aabbMax = center+extent; - + } void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const @@ -178,7 +219,9 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf btScalar totalMass = 0; btVector3 center(0, 0, 0); - for (int k = 0; k < n; k++) + int k; + + for (k = 0; k < n; k++) { center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; @@ -187,7 +230,7 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf principal.setOrigin(center); btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); - for (int k = 0; k < n; k++) + for ( k = 0; k < n; k++) { btVector3 i; m_children[k].m_childShape->calculateLocalInertia(masses[k], i); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 3624749f768..434860c8633 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -46,23 +46,26 @@ SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompou c1.m_childMargin == c2.m_childMargin ); } -/// btCompoundShape allows to store multiple other btCollisionShapes +/// 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. +/// @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 { - //btAlignedObjectArray<btTransform> m_childTransforms; - //btAlignedObjectArray<btCollisionShape*> m_childShapes; btAlignedObjectArray<btCompoundShapeChild> m_children; btVector3 m_localAabbMin; btVector3 m_localAabbMax; - //btOptimizedBvh* m_aabbTree; btDbvt* m_dynamicAabbTree; + ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated + int m_updateRevision; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btCompoundShape(); + btCompoundShape(bool enableDynamicAabbTree = true); virtual ~btCompoundShape(); @@ -88,15 +91,18 @@ 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); + btCompoundShapeChild* getChildList() { @@ -121,8 +127,6 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - virtual int getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} - virtual void setMargin(btScalar margin) { m_collisionMargin = margin; @@ -151,6 +155,10 @@ public: ///of the collision object by the principal transform. void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const; + int getUpdateRevision() const + { + return m_updateRevision; + } private: btScalar m_collisionMargin; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index f3f57206ab7..30065d55a12 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -20,6 +20,16 @@ subject to the following restrictions: #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 { + PHY_FLOAT, + PHY_DOUBLE, + PHY_INTEGER, + PHY_SHORT, + PHY_FIXEDPOINT88, + PHY_UCHAR +} PHY_ScalarType; ///The btConcaveShape class provides an interface for non-moving (static) concave shapes. ///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 207b3024bc3..d887be61ada 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -14,14 +14,14 @@ subject to the following restrictions: */ #include "btConeShape.h" -#include "LinearMath/btPoint3.h" -btConeShape::btConeShape (btScalar radius,btScalar height): +btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (), m_radius (radius), m_height(height) { + m_shapeType = CONE_SHAPE_PROXYTYPE; setConeUpIndex(1); btVector3 halfExtents; m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); @@ -60,7 +60,7 @@ void btConeShape::setConeUpIndex(int upIndex) m_coneIndices[2] = 1; break; default: - assert(0); + btAssert(0); }; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 685369742e4..1869893f37a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -69,9 +69,6 @@ public: } - - virtual int getShapeType() const { return CONE_SHAPE_PROXYTYPE; } - virtual const char* getName()const { return "Cone"; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 2596858bc3a..2b067367767 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -19,16 +19,17 @@ subject to the following restrictions: -btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) +btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexShape () { - m_points.resize(numPoints); + m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE; + m_unscaledPoints.resize(numPoints); unsigned char* pointsBaseAddress = (unsigned char*)points; for (int i=0;i<numPoints;i++) { - btPoint3* point = (btPoint3*)(pointsBaseAddress + i*stride); - m_points[i] = point[0]; + btVector3* point = (btVector3*)(pointsBaseAddress + i*stride); + m_unscaledPoints[i] = point[0]; } recalcLocalAabb(); @@ -43,9 +44,9 @@ void btConvexHullShape::setLocalScaling(const btVector3& scaling) recalcLocalAabb(); } -void btConvexHullShape::addPoint(const btPoint3& point) +void btConvexHullShape::addPoint(const btVector3& point) { - m_points.push_back(point); + m_unscaledPoints.push_back(point); recalcLocalAabb(); } @@ -67,9 +68,9 @@ btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVecto } - for (int i=0;i<m_points.size();i++) + for (int i=0;i<m_unscaledPoints.size();i++) { - btPoint3 vtx = m_points[i] * m_localScaling; + btVector3 vtx = m_unscaledPoints[i] * m_localScaling; newDot = vec.dot(vtx); if (newDot > maxDot) @@ -91,9 +92,9 @@ void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const supportVerticesOut[i][3] = btScalar(-1e30); } } - for (int i=0;i<m_points.size();i++) + for (int i=0;i<m_unscaledPoints.size();i++) { - btPoint3 vtx = m_points[i] * m_localScaling; + btVector3 vtx = getScaledPoint(i); for (int j=0;j<numVectors;j++) { @@ -144,26 +145,26 @@ btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo int btConvexHullShape::getNumVertices() const { - return m_points.size(); + return m_unscaledPoints.size(); } int btConvexHullShape::getNumEdges() const { - return m_points.size(); + return m_unscaledPoints.size(); } -void btConvexHullShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const +void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const { - int index0 = i%m_points.size(); - int index1 = (i+1)%m_points.size(); - pa = m_points[index0]*m_localScaling; - pb = m_points[index1]*m_localScaling; + int index0 = i%m_unscaledPoints.size(); + int index1 = (i+1)%m_unscaledPoints.size(); + pa = getScaledPoint(index0); + pb = getScaledPoint(index1); } -void btConvexHullShape::getVertex(int i,btPoint3& vtx) const +void btConvexHullShape::getVertex(int i,btVector3& vtx) const { - vtx = m_points[i]*m_localScaling; + vtx = getScaledPoint(i); } int btConvexHullShape::getNumPlanes() const @@ -171,16 +172,16 @@ int btConvexHullShape::getNumPlanes() const return 0; } -void btConvexHullShape::getPlane(btVector3& ,btPoint3& ,int ) const +void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const { btAssert(0); } //not yet -bool btConvexHullShape::isInside(const btPoint3& ,btScalar ) const +bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const { - assert(0); + btAssert(0); return false; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 4773de2dc51..baf074be6c3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -24,7 +24,7 @@ subject to the following restrictions: ///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex. ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape { - btAlignedObjectArray<btPoint3> m_points; + btAlignedObjectArray<btVector3> m_unscaledPoints; public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -33,23 +33,38 @@ public: ///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(btPoint3)); + btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3)); - void addPoint(const btPoint3& point); + void addPoint(const btVector3& point); - btPoint3* getPoints() + + btVector3* getUnscaledPoints() + { + return &m_unscaledPoints[0]; + } + + const btVector3* getUnscaledPoints() const + { + return &m_unscaledPoints[0]; + } + + ///getPoints is obsolete, please use getUnscaledPoints + const btVector3* getPoints() const { - return &m_points[0]; + return getUnscaledPoints(); } - const btPoint3* getPoints() const + + + + SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const { - return &m_points[0]; + return m_unscaledPoints[i] * m_localScaling; } - int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { - return m_points.size(); + return m_unscaledPoints.size(); } virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; @@ -57,7 +72,6 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getShapeType()const { return CONVEX_HULL_SHAPE_PROXYTYPE; } //debugging virtual const char* getName()const {return "Convex";} @@ -65,11 +79,11 @@ public: virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const; - virtual void getVertex(int i,btPoint3& vtx) 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,btPoint3& planeSupport,int i ) const; - virtual bool isInside(const btPoint3& pt,btScalar tolerance) 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); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp index fb81c8a5bde..bfb741310e1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -17,6 +17,7 @@ subject to the following restrictions: #include "btConvexInternalShape.h" + btConvexInternalShape::btConvexInternalShape() : m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), m_collisionMargin(CONVEX_DISTANCE_MARGIN) @@ -48,7 +49,8 @@ void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAa tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); minAabb[i] = tmp[i]-margin; } -}; +} + btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const @@ -70,6 +72,7 @@ btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)c return supVertex; #else + btAssert(0); return btVector3(0,0,0); #endif //__SPU__ diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index 9388ccf31f2..bab720d7b6d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -19,23 +19,18 @@ class btConvexInternalShape : public btConvexShape btScalar m_padding; + btConvexInternalShape(); + public: - btConvexInternalShape(); + virtual ~btConvexInternalShape() { } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; -#ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; - - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; -#endif //#ifndef __SPU__ const btVector3& getImplicitShapeDimensions() const { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp new file mode 100644 index 00000000000..4ab5d692945 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -0,0 +1,156 @@ +/* +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 "btConvexPointCloudShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "LinearMath/btQuaternion.h" + +void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; + recalcLocalAabb(); +} + +#ifndef __SPU__ +btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + + for (int i=0;i<m_numPoints;i++) + { + btVector3 vtx = getScaledPoint(i); + + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return supVec; +} + +void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + btScalar newDot; + //use 'w' component of supportVerticesOut? + { + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i][3] = btScalar(-1e30); + } + } + for (int i=0;i<m_numPoints;i++) + { + btVector3 vtx = getScaledPoint(i); + + for (int j=0;j<numVectors;j++) + { + const btVector3& vec = vectors[j]; + + newDot = vec.dot(vtx); + if (newDot > supportVerticesOut[j][3]) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = vtx; + supportVerticesOut[j][3] = newDot; + } + } + } + + + +} + + + +btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=btScalar(0.) ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; +} + + +#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 +{ + return m_numPoints; +} + +int btConvexPointCloudShape::getNumEdges() const +{ + return 0; +} + +void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const +{ + btAssert (0); +} + +void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const +{ + vtx = m_unscaledPoints[i]*m_localScaling; +} + +int btConvexPointCloudShape::getNumPlanes() const +{ + return 0; +} + +void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const +{ + + btAssert(0); +} + +//not yet +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 new file mode 100644 index 00000000000..7e1c13b29c1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -0,0 +1,96 @@ +/* +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_CONVEX_POINT_CLOUD_SHAPE_H +#define BT_CONVEX_POINT_CLOUD_SHAPE_H + +#include "btPolyhedralConvexShape.h" +#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 btPolyhedralConvexShape +{ + btVector3* m_unscaledPoints; + int m_numPoints; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) + { + m_localScaling = localScaling; + m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; + m_unscaledPoints = points; + m_numPoints = numPoints; + + if (computeAabb) + recalcLocalAabb(); + } + + void setPoints (btVector3* points, int numPoints, bool computeAabb = true) + { + m_unscaledPoints = points; + m_numPoints = numPoints; + + if (computeAabb) + recalcLocalAabb(); + } + + SIMD_FORCE_INLINE btVector3* getUnscaledPoints() + { + return m_unscaledPoints; + } + + SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const + { + return m_unscaledPoints; + } + + SIMD_FORCE_INLINE int getNumPoints() const + { + return m_numPoints; + } + + 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; +#endif + + + //debugging + virtual const char* getName()const {return "ConvexPointCloud";} + + 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; + + ///in case we receive negative scaling + virtual void setLocalScaling(const btVector3& scaling); +}; + + +#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 7afcccf8b03..7e67696f4c7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -14,5 +14,366 @@ subject to the following restrictions: */ #include "btConvexShape.h" +#include "btTriangleShape.h" +#include "btSphereShape.h" +#include "btCylinderShape.h" +#include "btCapsuleShape.h" +#include "btConvexHullShape.h" +#include "btConvexPointCloudShape.h" +btConvexShape::btConvexShape () +{ +} +btConvexShape::~btConvexShape() +{ + +} + + + +static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints, const btVector3& localScaling) +{ + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); + + btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + 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; + } + + + for (int i=0;i<numPoints;i++) + { + btVector3 vtx = points[i] * localScaling; + + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); +} + +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(); + + return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), + btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), + btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); + } + 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.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(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 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()); + + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btScalar halfHeight = capsuleShape->getHalfHeight(); + int capsuleUpAxis = capsuleShape->getUpAxis(); + + btScalar radius = capsuleShape->getRadius(); + btVector3 supVec(0,0,0); + + btScalar maxDot(btScalar(-1e30)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0,0,0); + pos[capsuleUpAxis] = halfHeight; + + //vtx = pos +vec*(radius); + vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); + newDot = vec.dot(vtx); + + + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0,0,0); + pos[capsuleUpAxis] = -halfHeight; + + //vtx = pos +vec*(radius); + vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); + 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: + { + 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: +#ifndef __SPU__ + return this->localGetSupportingVertexWithoutMargin (localDir); +#else + btAssert (0); +#endif + } + + // should never reach here + btAssert (0); + return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); +} + +btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const +{ + btVector3 localDirNorm = localDir; + if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + } + localDirNorm.normalize (); + + return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; +} + +/* TODO: This should be bumped up to btCollisionShape () */ +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 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 (); +#else + btAssert (0); +#endif + } + + // should never reach here + btAssert (0); + return btScalar(0.0f); +} + +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 = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + + 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 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 = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + aabbMin = center - extent; + aabbMax = center + extent; + } + break; + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; + btScalar margin = convexHullShape->getMarginNonVirtual(); + convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); + } + break; + default: +#ifndef __SPU__ + this->getAabb (t, aabbMin, aabbMax); +#else + btAssert (0); +#endif + break; + } + + // should never reach here + btAssert (0); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index e4eb7f6dbf6..0cc7b349521 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -24,8 +24,6 @@ subject to the following restrictions: #include "btCollisionMargin.h" #include "LinearMath/btAlignedAllocator.h" -//todo: get rid of this btConvexCastResult thing! -struct btConvexCastResult; #define MAX_PREFERRED_PENETRATION_DIRECTIONS 10 /// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc. @@ -38,20 +36,25 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); - virtual ~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__ + + 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 btVector3 localGetSupportingVertex(const btVector3& vec)const =0; -#ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; -#endif //#ifndef __SPU__ - ///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; @@ -69,6 +72,9 @@ public: virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0; + + + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 02a293e82ba..4bd986bb7f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -20,8 +20,9 @@ subject to the following restrictions: btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -:m_stridingMesh(meshInterface) +: btPolyhedralConvexShape(), m_stridingMesh(meshInterface) { + m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; if ( calcAabb ) recalcLocalAabb(); } @@ -107,7 +108,7 @@ void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargi } } - //todo: could do the batch inside the callback! + ///@todo: could do the batch inside the callback! for (int j=0;j<numVectors;j++) @@ -162,12 +163,12 @@ int btConvexTriangleMeshShape::getNumEdges() const return 0; } -void btConvexTriangleMeshShape::getEdge(int ,btPoint3& ,btPoint3& ) const +void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const { btAssert(0); } -void btConvexTriangleMeshShape::getVertex(int ,btPoint3& ) const +void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const { btAssert(0); } @@ -177,13 +178,13 @@ int btConvexTriangleMeshShape::getNumPlanes() const return 0; } -void btConvexTriangleMeshShape::getPlane(btVector3& ,btPoint3& ,int ) const +void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const { btAssert(0); } //not yet -bool btConvexTriangleMeshShape::isInside(const btPoint3& ,btScalar ) const +bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const { btAssert(0); return false; @@ -268,15 +269,12 @@ void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& pri btVector3 a = triangle[0] - center; btVector3 b = triangle[1] - center; btVector3 c = triangle[2] - center; - btVector3 abc = a + b + c; btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); for (int j = 0; j < 3; j++) { for (int k = 0; k <= j; k++) { - i[j][k] = i[k][j] = volNeg * (center[j] * center[k] - + btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j]) - + btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + 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])); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index 6ff0bf5d43b..9d7e39fce69 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -29,18 +29,16 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getShapeType()const { return CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; } - //debugging virtual const char* getName()const {return "ConvexTrimesh";} virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const; - virtual void getVertex(int i,btPoint3& vtx) 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,btPoint3& planeSupport,int i ) const; - virtual bool isInside(const btPoint3& pt,btScalar tolerance) 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); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 3afef1c7550..c9fa907edf3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -13,12 +13,12 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ #include "btCylinderShape.h" -#include "LinearMath/btPoint3.h" btCylinderShape::btCylinderShape (const btVector3& halfExtents) :btBoxShape(halfExtents), m_upAxis(1) { + m_shapeType = CYLINDER_SHAPE_PROXYTYPE; recalcLocalAabb(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h index 30938acf7c7..bda74a8612d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -62,11 +62,7 @@ public: //use box inertia // virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - virtual int getShapeType() const - { - return CYLINDER_SHAPE_PROXYTYPE; - } - + int getUpAxis() const { return m_upAxis; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index 6954aad7917..8387b9584bb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -19,8 +19,9 @@ subject to the following restrictions: #include "btCollisionShape.h" -btEmptyShape::btEmptyShape() +btEmptyShape::btEmptyShape() : btConcaveShape () { + m_shapeType = EMPTY_SHAPE_PROXYTYPE; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h index d7d78407b61..8720d53ae63 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -51,14 +51,14 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - virtual int getShapeType() const { return EMPTY_SHAPE_PROXYTYPE;} - - virtual const char* getName()const { return "Empty"; } + virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const + { + } protected: btVector3 m_localScaling; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index a291d6b7ce7..7d4875d8037 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -18,71 +18,107 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) -: m_heightStickWidth(heightStickWidth), -m_heightStickLength(heightStickLength), -m_maxHeight(maxHeight), -m_width((btScalar)heightStickWidth-1), -m_length((btScalar)heightStickLength-1), -m_heightfieldDataUnknown(heightfieldData), -m_useFloatData(useFloatData), -m_flipQuadEdges(flipQuadEdges), -m_useDiamondSubdivision(false), -m_upAxis(upAxis), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) + +btHeightfieldTerrainShape::btHeightfieldTerrainShape +( +int heightStickWidth, int heightStickLength, void* heightfieldData, +btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, +PHY_ScalarType hdt, bool flipQuadEdges +) { + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); +} + - btScalar quantizationMargin = 1.f; +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +{ + // legacy constructor: support only float or unsigned char, + // and min height is zero + PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; + btScalar minHeight = 0.0; + + // previously, height = uchar * maxHeight / 65535. + // So to preserve legacy behavior, heightScale = maxHeight / 65535 + btScalar heightScale = maxHeight / 65535; + + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); +} - //enlarge the AABB to avoid division by zero when initializing the quantization values - btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); - btVector3 halfExtents(0,0,0); +void btHeightfieldTerrainShape::initialize +( +int heightStickWidth, int heightStickLength, 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(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"); + + // initialize member variables + m_shapeType = TERRAIN_SHAPE_PROXYTYPE; + m_heightStickWidth = heightStickWidth; + m_heightStickLength = heightStickLength; + m_minHeight = minHeight; + m_maxHeight = maxHeight; + 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_upAxis = upAxis; + m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); + + // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) { case 0: { - halfExtents.setValue( - btScalar(m_maxHeight), - btScalar(m_width), //?? don't know if this should change - btScalar(m_length)); + m_localAabbMin.setValue(m_minHeight, 0, 0); + m_localAabbMax.setValue(m_maxHeight, m_width, m_length); break; } case 1: { - halfExtents.setValue( - btScalar(m_width), - btScalar(m_maxHeight), - btScalar(m_length)); + m_localAabbMin.setValue(0, m_minHeight, 0); + m_localAabbMax.setValue(m_width, m_maxHeight, m_length); break; }; case 2: { - halfExtents.setValue( - btScalar(m_width), - btScalar(m_length), - btScalar(m_maxHeight) - ); + m_localAabbMin.setValue(0, 0, m_minHeight); + m_localAabbMax.setValue(m_width, m_length, m_maxHeight); break; } default: { //need to get valid m_upAxis - btAssert(0); + btAssert(0 && "Bad m_upAxis"); } } - halfExtents*= btScalar(0.5); - - m_localAabbMin = -halfExtents - clampValue; - m_localAabbMax = halfExtents + clampValue; - btVector3 aabbSize = m_localAabbMax - m_localAabbMin; - + // remember origin (defined as exact middle of aabb) + m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); } + btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { } @@ -92,57 +128,80 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape() void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); - halfExtents += btVector3(getMargin(),getMargin(),getMargin()); + + btVector3 localOrigin(0, 0, 0); + localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); + localOrigin *= m_localScaling; btMatrix3x3 abs_b = t.getBasis().absolute(); - btPoint3 center = t.getOrigin(); + btVector3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents), abs_b[1].dot(halfExtents), abs_b[2].dot(halfExtents)); - + extent += btVector3(getMargin(),getMargin(),getMargin()); aabbMin = center - extent; aabbMax = center + extent; - - } -btScalar btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const + +/// 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 { btScalar val = 0.f; - if (m_useFloatData) + switch (m_heightDataType) { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; - } else - { - //assume unsigned short int - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; - val = heightFieldValue* (m_maxHeight/btScalar(65535)); + case PHY_FLOAT: + { + val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; + break; + } + + case PHY_UCHAR: + { + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; + val = heightFieldValue * m_heightScale; + break; + } + + case PHY_SHORT: + { + short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; + val = hfValue * m_heightScale; + break; + } + + default: + { + btAssert(!"Bad m_heightDataType"); + } } + return val; } - +/// this returns the vertex in bullet-local coordinates void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const { - btAssert(x>=0); btAssert(y>=0); btAssert(x<m_heightStickWidth); btAssert(y<m_heightStickLength); - - btScalar height = getHeightFieldValue(x,y); + btScalar height = getRawHeightFieldValue(x,y); switch (m_upAxis) { case 0: { vertex.setValue( - height, + height - m_localOrigin.getX(), (-m_width/btScalar(2.0)) + x, (-m_length/btScalar(2.0) ) + y ); @@ -152,7 +211,7 @@ void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const { vertex.setValue( (-m_width/btScalar(2.0)) + x, - height, + height - m_localOrigin.getY(), (-m_length/btScalar(2.0)) + y ); break; @@ -162,7 +221,7 @@ void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const vertex.setValue( (-m_width/btScalar(2.0)) + x, (-m_length/btScalar(2.0)) + y, - height + height - m_localOrigin.getZ() ); break; } @@ -174,45 +233,76 @@ void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const } vertex*=m_localScaling; - } + +static inline int +getQuantized +( +btScalar x +) +{ + if (x < 0.0) { + 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 + input vector, answering the question: "which gridpoint is closest to the + provided point?". + + "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 { btVector3 clampedPoint(point); clampedPoint.setMax(m_localAabbMin); clampedPoint.setMin(m_localAabbMax); - btVector3 v = (clampedPoint);// - m_bvhAabbMin) * m_bvhQuantization; - - //TODO: optimization: check out how to removed this btFabs + out[0] = getQuantized(clampedPoint.getX()); + out[1] = getQuantized(clampedPoint.getY()); + out[2] = getQuantized(clampedPoint.getZ()); - out[0] = (int)(v.getX() + v.getX() / btFabs(v.getX())* btScalar(0.5) ); - out[1] = (int)(v.getY() + v.getY() / btFabs(v.getY())* btScalar(0.5) ); - out[2] = (int)(v.getZ() + v.getZ() / btFabs(v.getZ())* btScalar(0.5) ); - } + +/// process all triangles within the provided axis-aligned bounding box +/** + basic algorithm: + - convert input aabb to local coordinates (scale down and shift for local origin) + - 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)callback; - (void)aabbMax; - (void)aabbMin; + // 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]); - //quantize the aabbMin and aabbMax, and adjust the start/end ranges + // 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]; - - btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); - btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); - quantizeWithClamp(quantizedAabbMin, localAabbMin,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) { + quantizedAabbMin[i]--; + quantizedAabbMax[i]++; + } int startX=0; int endX=m_heightStickWidth-1; @@ -223,11 +313,6 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback { case 0: { - quantizedAabbMin[1]+=m_heightStickWidth/2-1; - quantizedAabbMax[1]+=m_heightStickWidth/2+1; - quantizedAabbMin[2]+=m_heightStickLength/2-1; - quantizedAabbMax[2]+=m_heightStickLength/2+1; - if (quantizedAabbMin[1]>startX) startX = quantizedAabbMin[1]; if (quantizedAabbMax[1]<endX) @@ -240,11 +325,6 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } case 1: { - quantizedAabbMin[0]+=m_heightStickWidth/2-1; - quantizedAabbMax[0]+=m_heightStickWidth/2+1; - quantizedAabbMin[2]+=m_heightStickLength/2-1; - quantizedAabbMax[2]+=m_heightStickLength/2+1; - if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0]<endX) @@ -257,11 +337,6 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback }; case 2: { - quantizedAabbMin[0]+=m_heightStickWidth/2-1; - quantizedAabbMax[0]+=m_heightStickWidth/2+1; - quantizedAabbMin[1]+=m_heightStickLength/2-1; - quantizedAabbMax[1]+=m_heightStickLength/2+1; - if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0]<endX) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index d6890fe6b97..36489a0ebc3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -18,28 +18,80 @@ subject to the following restrictions: #include "btConcaveShape.h" -///The btHeightfieldTerrainShape simulates a 2D heightfield terrain collision shape. You can also use the more general btBvhTriangleMeshShape instead. -///An example implementation of btHeightfieldTerrainShape is provided in Demos/VehicleDemo/VehicleDemo.cpp +///btHeightfieldTerrainShape simulates a 2D heightfield terrain +/** + The caller is responsible for maintaining the heightfield array; this + class does not make a copy. + + The heightfield can be dynamic so long as the min/max height values + capture the extremes (heights must always be in that range). + + The local origin of the heightfield is assumed to be the exact + center (as determined by width and length and height, with each + axis multiplied by the localScaling). + + \b NOTE: be careful with coordinates. If you have a heightfield with a local + min height of -100m, and a max height of +500m, you may be tempted to place it + at the origin (0,0) and expect the heights in world coordinates to be + -100 to +500 meters. + Actually, the heights will be -300 to +300m, because bullet will re-center + the heightfield based on its AABB (which is determined by the min/max + heights). So keep in mind that once you create a btHeightfieldTerrainShape + object, the heights will be adjusted relative to the center of the AABB. This + is different to the behavior of many rendering engines, but is useful for + physics engines. + + Most (but not all) rendering and heightfield libraries assume upAxis = 1 + (that is, the y-axis is "up"). This class allows any of the 3 coordinates + to be "up". Make sure your choice of axis is consistent with your rendering + system. + + The heightfield heights are determined from the data type used for the + heightfieldData array. + + - PHY_UCHAR: height at a point is the uchar value at the + grid point, multipled by heightScale. uchar isn't recommended + because of its inability to deal with negative values, and + low resolution (8-bit). + + - PHY_SHORT: height at a point is the short int value at that grid + point, multipled by heightScale. + + - PHY_FLOAT: height at a point is the float value at that grid + point. heightScale is ignored when using the float heightfield + data type. + + Whatever the caller specifies as minHeight and maxHeight will be honored. + The class will not inspect the heightfield to discover the actual minimum + or maximum heights. These values are used to determine the heightfield's + axis-aligned bounding box, multiplied by localScaling. + + For usage and testing see the TerrainDemo. + */ class btHeightfieldTerrainShape : public btConcaveShape { protected: btVector3 m_localAabbMin; btVector3 m_localAabbMax; - + btVector3 m_localOrigin; + ///terrain data int m_heightStickWidth; int m_heightStickLength; + btScalar m_minHeight; btScalar m_maxHeight; btScalar m_width; btScalar m_length; + btScalar m_heightScale; union { unsigned char* m_heightfieldDataUnsignedChar; + short* m_heightfieldDataShort; btScalar* m_heightfieldDataFloat; void* m_heightfieldDataUnknown; }; - - bool m_useFloatData; + + PHY_ScalarType m_heightDataType; bool m_flipQuadEdges; bool m_useDiamondSubdivision; @@ -47,31 +99,49 @@ protected: btVector3 m_localScaling; - virtual btScalar getHeightFieldValue(int x,int y) const; + 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; - inline bool testQuantizedAabbAgainstQuantizedAabb(int* aabbMin1, int* aabbMax1,const int* aabbMin2,const int* aabbMax2) const - { - bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } + + + /// protected initialization + /** + Handles the work of constructors so that public constructors can be + backwards-compatible without a lot of copy/paste. + */ + void initialize(int heightStickWidth, int heightStickLength, + void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType heightDataType, bool flipQuadEdges); public: - btHeightfieldTerrainShape(int heightStickWidth,int heightStickHeight,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + /// 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, + void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, PHY_ScalarType heightDataType, + bool flipQuadEdges); + + /// legacy constructor + /** + The legacy constructor assumes the heightfield has a minimum height + of zero. Only unsigned char or floats are supported. For legacy + compatibility reasons, heightScale is calculated as maxHeight / 65535 + (and is only used when useFloatData = false). + */ + btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} - virtual int getShapeType() const - { - return TERRAIN_SHAPE_PROXYTYPE; - } virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 8f893ac98aa..b107b0bea6e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -17,23 +17,25 @@ subject to the following restrictions: btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB) -:m_shapeA(shapeA), +: 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 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis())); - btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis())); + btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); + btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis())); return supVertexA - supVertexB; } 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. + ///@todo: could make recursive use of batching. probably this shape is not used frequently. for (int i=0;i<numVectors;i++) { supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index f9c8427c2f3..6f637fbf40b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -46,8 +46,6 @@ public: const btTransform& GetTransformB()const { return m_transB;} - virtual int getShapeType() const { return MINKOWSKI_SUM_SHAPE_PROXYTYPE; } - virtual btScalar getMargin() const; const btConvexShape* getShapeA() const { return m_shapeA;} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index c709205efc2..9423505290d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -18,8 +18,9 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres) -:m_inertiaHalfExtents(inertiaHalfExtents) +:btConvexInternalShape (), m_inertiaHalfExtents(inertiaHalfExtents) { + m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE; btScalar startMargin = btScalar(1e30); m_numSpheres = numSpheres; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 45968f3947f..97309fbbed9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -62,7 +62,6 @@ public: return m_radi[index]; } - virtual int getShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; } virtual const char* getName()const { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h index 2f108020cdf..d025a5c4450 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -31,10 +31,12 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {} + btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;} btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + btVector3 m_triangle[3]; const unsigned char *vertexbase; int numverts; @@ -67,6 +69,8 @@ public: 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; + btVector3 m_triangle[3]; const unsigned char *vertexbase; int numverts; @@ -107,11 +111,6 @@ public: m_materialLookup = NULL; */ } - virtual int getShapeType() const - { - return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - } - //debugging virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index a248c55b571..3e73059394c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -319,19 +319,19 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f { int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); -#ifdef DEBUG_PATCH_COLORS - btVector3 mycolor = color[index&3]; - graphicsbase[8] = mycolor.getX(); - graphicsbase[9] = mycolor.getY(); - graphicsbase[10] = mycolor.getZ(); -#endif //DEBUG_PATCH_COLORS - - - triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + 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())); + } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 71c3af38a02..7a79ac25791 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -15,8 +15,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -btPolyhedralConvexShape::btPolyhedralConvexShape() -:m_localAabbMin(1,1,1), +btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), +m_localAabbMin(1,1,1), m_localAabbMax(-1,-1,-1), m_isLocalAabbValid(false), m_optionalHull(0) @@ -25,7 +25,6 @@ m_optionalHull(0) } - btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { int i; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 4c4ce7feaa7..7567a42d3ba 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BU_SHAPE #define BU_SHAPE -#include "LinearMath/btPoint3.h" #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAabbUtil2.h" #include "btConvexInternalShape.h" @@ -36,12 +35,28 @@ public: btPolyhedralConvexShape(); //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; + 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 + { + btAssert(m_isLocalAabbValid); + aabbMin = m_localAabbMin; + aabbMax = m_localAabbMax; + } + inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const { @@ -59,13 +74,13 @@ public: virtual int getNumVertices() const = 0 ; virtual int getNumEdges() const = 0; - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const = 0; - virtual void getVertex(int i,btPoint3& vtx) 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,btPoint3& planeSupport,int i ) const = 0; + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0; // virtual int getIndex(int i) const = 0 ; - virtual bool isInside(const btPoint3& pt,btScalar tolerance) const = 0; + virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0; /// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp class Hull* m_optionalHull; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp index 2b81a02b557..1fac1538769 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -15,11 +15,10 @@ subject to the following restrictions: #include "btScaledBvhTriangleMeshShape.h" -btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling) -:m_bvhTriMeshShape(childShape), -m_localScaling(localScaling) +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) +:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) { - + m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; } btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() @@ -35,7 +34,7 @@ class btScaledTriangleCallback : public btTriangleCallback public: - btScaledTriangleCallback(btTriangleCallback* originalCallback,btVector3 localScaling) + btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) :m_originalCallback(originalCallback), m_localScaling(localScaling) { @@ -94,7 +93,7 @@ void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& a btMatrix3x3 abs_b = trans.getBasis().absolute(); - btPoint3 center = trans(localCenter); + btVector3 center = trans(localCenter); btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), abs_b[1].dot(localHalfExtents), diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h index 0a1ef8ce5cb..5da6fe8bc95 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -32,15 +32,10 @@ ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape public: - btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling); + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling); virtual ~btScaledBvhTriangleMeshShape(); - virtual int getShapeType() const - { - //use un-used 'FAST_CONCAVE_MESH_PROXYTYPE' for now, later add SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE to btBroadphaseProxy.h - return SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; - } virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual void setLocalScaling(const btVector3& scaling); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp index 15cfe432e27..cab7b277289 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -18,12 +18,6 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" - -btSphereShape ::btSphereShape (btScalar radius) -{ - m_implicitShapeDimensions.setX(radius); -} - btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { (void)vec; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h index a6d01cdca9d..bf163d3b501 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -27,8 +27,12 @@ ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btSphereShape (btScalar radius); - + btSphereShape (btScalar radius) : btConvexInternalShape () + { + m_shapeType = SPHERE_SHAPE_PROXYTYPE; + m_implicitShapeDimensions.setX(radius); + m_collisionMargin = radius; + } virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; @@ -40,7 +44,6 @@ public: virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual int getShapeType() const { return SPHERE_SHAPE_PROXYTYPE; } btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index 3c98ca08ddc..17b8776d459 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -19,10 +19,11 @@ subject to the following restrictions: btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant) -:m_planeNormal(planeNormal.normalized()), +: btConcaveShape (), m_planeNormal(planeNormal.normalized()), m_planeConstant(planeConstant), m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.)) { + m_shapeType = STATIC_PLANE_PROXYTYPE; // btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) ); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index ddebca73dea..a496013c077 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -36,11 +36,6 @@ public: virtual ~btStaticPlaneShape(); - virtual int getShapeType() const - { - return STATIC_PLANE_PROXYTYPE; - } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index 85020dd3f58..e61b11499d4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -18,16 +18,9 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "btTriangleCallback.h" +#include "btConcaveShape.h" + -/// PHY_ScalarType enumerates possible scalar types. -/// See the btStridingMeshInterface for its use -typedef enum PHY_ScalarType { - PHY_FLOAT, - PHY_DOUBLE, - PHY_INTEGER, - PHY_SHORT, - PHY_FIXEDPOINT88 -} PHY_ScalarType; /// 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. @@ -77,8 +70,16 @@ class btStridingMeshInterface virtual void preallocateIndices(int numindices)=0; virtual bool hasPremadeAabb() const { return false; } - virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const {} - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const {} + 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; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 3aa1eda9964..7fd8fb4691b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -16,35 +16,40 @@ subject to the following restrictions: #include "btTetrahedronShape.h" #include "LinearMath/btMatrix3x3.h" -btBU_Simplex1to4::btBU_Simplex1to4() -:m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexShape (), +m_numVertices(0) { + m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; } -btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0) -:m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexShape (), +m_numVertices(0) { + m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); } -btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1) -:m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexShape (), +m_numVertices(0) { + m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); addVertex(pt1); } -btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2) -:m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexShape (), +m_numVertices(0) { + m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); addVertex(pt1); addVertex(pt2); } -btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3) -:m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexShape (), +m_numVertices(0) { + m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); addVertex(pt1); addVertex(pt2); @@ -55,7 +60,7 @@ btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const -void btBU_Simplex1to4::addVertex(const btPoint3& pt) +void btBU_Simplex1to4::addVertex(const btVector3& pt) { m_vertices[m_numVertices++] = pt; @@ -87,7 +92,7 @@ int btBU_Simplex1to4::getNumEdges() const return 0; } -void btBU_Simplex1to4::getEdge(int i,btPoint3& pa,btPoint3& pb) const +void btBU_Simplex1to4::getEdge(int i,btVector3& pa,btVector3& pb) const { switch (m_numVertices) @@ -151,7 +156,7 @@ void btBU_Simplex1to4::getEdge(int i,btPoint3& pa,btPoint3& pb) const } -void btBU_Simplex1to4::getVertex(int i,btPoint3& vtx) const +void btBU_Simplex1to4::getVertex(int i,btVector3& vtx) const { vtx = m_vertices[i]; } @@ -178,7 +183,7 @@ int btBU_Simplex1to4::getNumPlanes() const } -void btBU_Simplex1to4::getPlane(btVector3&, btPoint3& ,int ) const +void btBU_Simplex1to4::getPlane(btVector3&, btVector3& ,int ) const { } @@ -188,7 +193,7 @@ int btBU_Simplex1to4::getIndex(int ) const return 0; } -bool btBU_Simplex1to4::isInside(const btPoint3& ,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 09272d61dc2..114ae288bb0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -27,15 +27,15 @@ class btBU_Simplex1to4 : public btPolyhedralConvexShape protected: int m_numVertices; - btPoint3 m_vertices[4]; + btVector3 m_vertices[4]; public: btBU_Simplex1to4(); - btBU_Simplex1to4(const btPoint3& pt0); - btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1); - btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2); - btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3); + 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); void reset() @@ -44,9 +44,8 @@ public: } - virtual int getShapeType() const{ return TETRAHEDRAL_SHAPE_PROXYTYPE; } - void addVertex(const btPoint3& pt); + void addVertex(const btVector3& pt); //PolyhedralConvexShape interface @@ -54,17 +53,17 @@ public: virtual int getNumEdges() const; - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const; + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btPoint3& vtx) const; + virtual void getVertex(int i,btVector3& vtx) const; virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const; + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const; virtual int getIndex(int i) const; - virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; + virtual bool isInside(const btVector3& pt,btScalar tolerance) const; ///getName is for debugging diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index c739fc11e8f..adca38041b2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -28,7 +28,7 @@ struct btTriangle int m_triangleIndex; }; -///btTriangleBuffer can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' +///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' ///Example usage of this class: /// btTriangleBuffer triBuf; /// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index b962829041a..5f292532861 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -82,7 +82,8 @@ bool btTriangleIndexVertexArray::hasPremadeAabb() const return (m_hasAabb == 1); } -void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) + +void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const { m_aabbMin = aabbMin; m_aabbMax = aabbMax; @@ -95,3 +96,4 @@ void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* a *aabbMax = m_aabbMax; } + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index e546675f802..eb79ff5e47d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -52,9 +52,9 @@ ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshIn protected: IndexedMeshArray m_indexedMeshes; int m_pad[2]; - int m_hasAabb; // using int instead of bool to maintain alignment - btVector3 m_aabbMin; - btVector3 m_aabbMax; + mutable int m_hasAabb; // using int instead of bool to maintain alignment + mutable btVector3 m_aabbMin; + mutable btVector3 m_aabbMax; public: @@ -106,7 +106,7 @@ public: virtual void preallocateIndices(int numindices){(void) numindices;} virtual bool hasPremadeAabb() const; - virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ); + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const; virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp index 492854ff646..abefa71250a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -1,3 +1,4 @@ + /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 29d26316316..5dcfa51c280 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -35,13 +35,13 @@ m_weldingThreshold(0.0) if (m_use32bitIndices) { m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3; - m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0]; + m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_INTEGER; m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int); } else { m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3; - m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0]; + m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_SHORT; m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int); } @@ -49,12 +49,12 @@ m_weldingThreshold(0.0) if (m_use4componentVertices) { m_indexedMeshes[0].m_numVertices = m_4componentVertices.size(); - m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0]; + m_indexedMeshes[0].m_vertexBase = 0; m_indexedMeshes[0].m_vertexStride = sizeof(btVector3); } else { m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3; - m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; + m_indexedMeshes[0].m_vertexBase = 0; m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar); } @@ -74,17 +74,21 @@ void btTriangleMesh::addIndex(int index) } } -int btTriangleMesh::findOrAddVertex(const btVector3& vertex) + +int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices) { //return index of new/existing vertex - //todo: could use acceleration structure for this + ///@todo: could use acceleration structure for this if (m_use4componentVertices) { - for (int i=0;i< m_4componentVertices.size();i++) - { - if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold) + if (removeDuplicateVertices) + { + for (int i=0;i< m_4componentVertices.size();i++) { - return i; + if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold) + { + return i; + } } } m_indexedMeshes[0].m_numVertices++; @@ -96,17 +100,20 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex) } else { - for (int i=0;i< m_3componentVertices.size();i+=3) + if (removeDuplicateVertices) { - btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]); - if ((vtx-vertex).length2() <= m_weldingThreshold) + for (int i=0;i< m_3componentVertices.size();i+=3) { - return i/3; + btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]); + if ((vtx-vertex).length2() <= m_weldingThreshold) + { + return i/3; + } } - } - m_3componentVertices.push_back(vertex.getX()); - m_3componentVertices.push_back(vertex.getY()); - m_3componentVertices.push_back(vertex.getZ()); + } + m_3componentVertices.push_back((float)vertex.getX()); + m_3componentVertices.push_back((float)vertex.getY()); + m_3componentVertices.push_back((float)vertex.getZ()); m_indexedMeshes[0].m_numVertices++; m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; return (m_3componentVertices.size()/3)-1; @@ -114,13 +121,12 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex) } -void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2) +void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2,bool removeDuplicateVertices) { m_indexedMeshes[0].m_numTriangles++; - - addIndex(findOrAddVertex(vertex0)); - addIndex(findOrAddVertex(vertex1)); - addIndex(findOrAddVertex(vertex2)); + addIndex(findOrAddVertex(vertex0,removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex1,removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex2,removeDuplicateVertices)); } int btTriangleMesh::getNumTriangles() const diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 1f51b2f2c87..f06fbb7c22d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -25,7 +25,6 @@ subject to the following restrictions: ///It allows either 32bit or 16bit indices, and 4 (x-y-z-w) or 3 (x-y-z) component vertices. ///If you want to share triangle/index data between graphics mesh and collision mesh (btBvhTriangleMeshShape), you can directly use btTriangleIndexVertexArray or derive your own class from btStridingMeshInterface. ///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same. -///It has a brute-force option to weld together closeby vertices. class btTriangleMesh : public btTriangleIndexVertexArray { btAlignedObjectArray<btVector3> m_4componentVertices; @@ -42,7 +41,7 @@ class btTriangleMesh : public btTriangleIndexVertexArray btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); - int findOrAddVertex(const btVector3& vertex); + int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); void addIndex(int index); bool getUse32bitIndices() const @@ -54,8 +53,9 @@ class btTriangleMesh : public btTriangleIndexVertexArray { return m_use4componentVertices; } - - void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2); + ///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); int getNumTriangles() const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index 0a2c77096d1..1bd7e84a4e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -22,8 +22,9 @@ subject to the following restrictions: btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) -: m_meshInterface(meshInterface) +: btConcaveShape (), m_meshInterface(meshInterface) { + m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; if(meshInterface->hasPremadeAabb()) { meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax); @@ -52,7 +53,7 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt btMatrix3x3 abs_b = trans.getBasis().absolute(); - btPoint3 center = trans(localCenter); + btVector3 center = trans(localCenter); btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), abs_b[1].dot(localHalfExtents), diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index c9eabafe290..53d65799001 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -40,7 +40,7 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - assert(0); + btAssert(0); return localGetSupportingVertex(vec); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index ba7aa0c32a0..4bfb0b9e881 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -40,17 +40,13 @@ public: { vert = m_vertices1[index]; } - virtual int getShapeType() const - { - return TRIANGLE_SHAPE_PROXYTYPE; - } virtual int getNumEdges() const { return 3; } - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const { getVertex(i,pa); getVertex((i+1)%3,pb); @@ -83,15 +79,16 @@ public: - btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) + 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; } - virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const { getPlaneEquation(i,planeNormal,planeSupport); } @@ -107,7 +104,7 @@ public: normal.normalize(); } - virtual void getPlaneEquation(int i, btVector3& planeNormal,btPoint3& planeSupport) const + virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const { (void)i; calcNormal(planeNormal); @@ -121,7 +118,7 @@ public: inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } - virtual bool isInside(const btPoint3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt,btScalar tolerance) const { btVector3 normal; calcNormal(normal); @@ -135,7 +132,7 @@ public: int i; for (i=0;i<3;i++) { - btPoint3 pa,pb; + btVector3 pa,pb; getEdge(i,pa,pb); btVector3 edge = pb-pa; btVector3 edgeNormal = edge.cross(normal); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp index ef340286cb0..4c1c716d519 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -16,9 +16,10 @@ subject to the following restrictions: #include "btUniformScalingShape.h" btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor): -m_childConvexShape(convexChildShape), +btConvexShape (), m_childConvexShape(convexChildShape), m_uniformScalingFactor(uniformScalingFactor) { + m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE; } btUniformScalingShape::~btUniformScalingShape() diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h index 1e17fc8e198..945976ac036 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -61,7 +61,6 @@ class btUniformScalingShape : public btConvexShape return "UniformScalingShape"; } - virtual int getShapeType() const { return UNIFORM_SCALING_SHAPE_PROXYTYPE; } /////////////////////////// diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h index cf43f487e24..827a3c89587 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h @@ -26,9 +26,6 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" -/*! \defgroup BOUND_AABB_OPERATIONS -*/ -//! @{ ///Swap numbers #define BT_SWAP_NUMBERS(a,b){ \ @@ -41,7 +38,7 @@ subject to the following restrictions: #define BT_MAX(a,b) (a<b?b:a) #define BT_MIN(a,b) (a>b?b:a) -#define BT_GREATER(x, y) fabsf(x) > (y) +#define BT_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)) @@ -181,7 +178,7 @@ public: { for(j=0;j<3;j++ ) { - m_AR[i][j] = 1e-6f + fabsf(m_R1to0[i][j]); + m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); } } @@ -646,6 +643,5 @@ SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btT } -//! @} #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 28c03b7e039..5de391a7561 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h @@ -27,9 +27,6 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btGeometryUtil.h" -/*! \addtogroup GEOMETRIC_OPERATIONS -*/ -//! @{ SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point) { @@ -181,6 +178,5 @@ SIMD_FORCE_INLINE int bt_plane_clip_triangle( -//! @} #endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp index faba2d8a542..c3b697bdd1e 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp @@ -103,12 +103,12 @@ void btContactArray::merge_contacts( push_back(contacts[keycontacts[0].m_value]); - BT_CONTACT * pcontact = &(*this)[0]; + GIM_CONTACT * pcontact = &(*this)[0]; for( i=1;i<keycontacts.size();i++) { key = keycontacts[i].m_key; - const BT_CONTACT * scontact = &contacts[keycontacts[i].m_value]; + const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; if(last_key == key)//same points { @@ -158,7 +158,7 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts) return; } - BT_CONTACT average_contact = contacts[0]; + GIM_CONTACT average_contact = contacts[0]; for (int i=1;i<contacts.size() ;i++ ) { diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h index 0cfd013b962..4b0b70293b4 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h @@ -29,11 +29,6 @@ subject to the following restrictions: #include "btTriangleShapeEx.h" -/*! \defgroup CONTACTS -\brief -Functions for managing and sorting contacts resulting from a collision query. -*/ -//! @{ /** Configuration var for applying interpolation of contact normals @@ -42,8 +37,9 @@ Configuration var for applying interpolation of contact normals #define CONTACT_DIFF_EPSILON 0.00001f -/// Structure for collision results -class BT_CONTACT +///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; @@ -53,11 +49,11 @@ public: int m_feature1;//Face number int m_feature2;//Face number public: - BT_CONTACT() + GIM_CONTACT() { } - BT_CONTACT(const BT_CONTACT & contact): + GIM_CONTACT(const GIM_CONTACT & contact): m_point(contact.m_point), m_normal(contact.m_normal), m_depth(contact.m_depth), @@ -66,7 +62,7 @@ public: { } - BT_CONTACT(const btVector3 &point,const btVector3 & normal, + GIM_CONTACT(const btVector3 &point,const btVector3 & normal, btScalar depth, int feature1, int feature2): m_point(point), m_normal(normal), @@ -112,7 +108,7 @@ public: }; -class btContactArray:public btAlignedObjectArray<BT_CONTACT> +class btContactArray:public btAlignedObjectArray<GIM_CONTACT> { public: btContactArray() @@ -124,11 +120,11 @@ public: const btVector3 &point,const btVector3 & normal, btScalar depth, int feature1, int feature2) { - push_back( BT_CONTACT(point,normal,depth,feature1,feature2) ); + push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) ); } SIMD_FORCE_INLINE void push_triangle_contacts( - const BT_TRIANGLE_CONTACT & tricontact, + const GIM_TRIANGLE_CONTACT & tricontact, int feature1,int feature2) { for(int i = 0;i<tricontact.m_point_count ;i++ ) @@ -145,5 +141,5 @@ public: void merge_contacts_unique(const btContactArray & contacts); }; -//! @} + #endif // GIM_CONTACT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp index b07926c30dc..38fed6b4fd2 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp @@ -65,7 +65,7 @@ float btGImpactBvh::getAverageTreeCollisionTime() /////////////////////// btBvhTree ///////////////////////////////// int btBvhTree::_calc_splitting_axis( - BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) { int i; @@ -97,7 +97,7 @@ int btBvhTree::_calc_splitting_axis( int btBvhTree::_sort_and_calc_splitting_index( - BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; @@ -150,15 +150,14 @@ int btBvhTree::_sort_and_calc_splitting_index( splitIndex = startIndex+ (numIndices>>1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - btAssert(!unbal); + btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); return splitIndex; } -void btBvhTree::_build_sub_tree(BT_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++; @@ -211,7 +210,7 @@ void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIn //! stackless build tree void btBvhTree::build_tree( - BT_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY & primitive_boxes) { // initialize node count to 0 m_num_nodes = 0; @@ -236,7 +235,7 @@ void btGImpactBvh::refit() } else { - //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); //get left bound btAABB bound; bound.invalidate(); @@ -266,7 +265,7 @@ void btGImpactBvh::refit() void btGImpactBvh::buildSet() { //obtain primitive boxes - BT_BVH_DATA_ARRAY primitive_boxes; + GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); for (int i = 0;i<primitive_boxes.size() ;i++ ) diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h index 1907efc2463..b432b615f8a 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h @@ -32,29 +32,23 @@ subject to the following restrictions: -/*! \defgroup BOX_TREES - - - -*/ -//! @{ //! Overlapping pair -struct BT_PAIR +struct GIM_PAIR { int m_index1; int m_index2; - BT_PAIR() + GIM_PAIR() {} - BT_PAIR(const BT_PAIR & p) + GIM_PAIR(const GIM_PAIR & p) { m_index1 = p.m_index1; m_index2 = p.m_index2; } - BT_PAIR(int index1, int index2) + GIM_PAIR(int index1, int index2) { m_index1 = index1; m_index2 = index2; @@ -62,7 +56,7 @@ struct BT_PAIR }; //! A pairset array -class btPairSet: public btAlignedObjectArray<BT_PAIR> +class btPairSet: public btAlignedObjectArray<GIM_PAIR> { public: btPairSet() @@ -71,32 +65,32 @@ public: } inline void push_pair(int index1,int index2) { - push_back(BT_PAIR(index1,index2)); + push_back(GIM_PAIR(index1,index2)); } inline void push_pair_inv(int index1,int index2) { - push_back(BT_PAIR(index2,index1)); + push_back(GIM_PAIR(index2,index1)); } }; - -struct BT_BVH_DATA +///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 BT_BVH_TREE_NODE +class GIM_BVH_TREE_NODE { public: btAABB m_bound; protected: int m_escapeIndexOrDataIndex; public: - BT_BVH_TREE_NODE() + GIM_BVH_TREE_NODE() { m_escapeIndexOrDataIndex = 0; } @@ -133,12 +127,12 @@ public: }; -class BT_BVH_DATA_ARRAY:public btAlignedObjectArray<BT_BVH_DATA> +class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA> { }; -class BT_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<BT_BVH_TREE_NODE> +class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<GIM_BVH_TREE_NODE> { }; @@ -150,15 +144,15 @@ class btBvhTree { protected: int m_num_nodes; - BT_BVH_TREE_NODE_ARRAY m_node_array; + GIM_BVH_TREE_NODE_ARRAY m_node_array; protected: int _sort_and_calc_splitting_index( - BT_BVH_DATA_ARRAY & primitive_boxes, + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(BT_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() { @@ -167,7 +161,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -218,7 +212,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const BT_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]; } @@ -382,12 +376,11 @@ public: } - SIMD_FORCE_INLINE const BT_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); } -//! @} static float getAverageTreeCollisionTime(); diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp index 28ad18fbde2..4d6fa88f62d 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -192,7 +192,7 @@ float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) -: btCollisionAlgorithm(ci) +: btActivatingCollisionAlgorithm(ci,body0,body1) { m_manifoldPtr = NULL; m_convex_algorithm = NULL; @@ -404,7 +404,7 @@ void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body btPrimitiveTriangle ptri0; btPrimitiveTriangle ptri1; - BT_TRIANGLE_CONTACT contact_data; + GIM_TRIANGLE_CONTACT contact_data; shape0->lockChildShapes(); shape1->lockChildShapes(); @@ -540,7 +540,7 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( int i = pairset.size(); while(i--) { - BT_PAIR * pair = &pairset[i]; + GIM_PAIR * pair = &pairset[i]; m_triface0 = pair->m_index1; m_triface1 = pair->m_index2; btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); @@ -584,14 +584,15 @@ void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0, if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) { btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0); - m_part0 = meshshape0->getMeshPartCount(); + int& part = swapped ? m_part1 : m_part0; + part = meshshape0->getMeshPartCount(); - while(m_part0--) + while(part--) { gimpact_vs_shape(body0, body1, - meshshape0->getMeshPart(m_part0), + meshshape0->getMeshPart(part), shape1,swapped); } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index c745b7ed5d8..453472aa034 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -24,7 +24,7 @@ subject to the following restrictions: #ifndef BVH_CONCAVE_COLLISION_ALGORITHM_H #define BVH_CONCAVE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -51,7 +51,7 @@ btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(m_dyna btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); \endcode */ -class btGImpactCollisionAlgorithm : public btCollisionAlgorithm +class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm { protected: btCollisionAlgorithm * m_convex_algorithm; @@ -67,7 +67,7 @@ protected: //! Creates a new contact point SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(btCollisionObject* body0,btCollisionObject* body1) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); return m_manifoldPtr; } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp index 0b1ef6acd83..ea1647a8129 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -67,7 +67,7 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime() /////////////////////// btQuantizedBvhTree ///////////////////////////////// void btQuantizedBvhTree::calc_quantization( - BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) + GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) { //calc globa box btAABB global_bound; @@ -86,7 +86,7 @@ void btQuantizedBvhTree::calc_quantization( int btQuantizedBvhTree::_calc_splitting_axis( - BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) { int i; @@ -118,7 +118,7 @@ int btQuantizedBvhTree::_calc_splitting_axis( int btQuantizedBvhTree::_sort_and_calc_splitting_index( - BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; @@ -171,15 +171,14 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index( splitIndex = startIndex+ (numIndices>>1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - btAssert(!unbal); + btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); return splitIndex; } -void btQuantizedBvhTree::_build_sub_tree(BT_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++; @@ -232,7 +231,7 @@ void btQuantizedBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, in //! stackless build tree void btQuantizedBvhTree::build_tree( - BT_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY & primitive_boxes) { calc_quantization(primitive_boxes); // initialize node count to 0 @@ -258,7 +257,7 @@ void btGImpactQuantizedBvh::refit() } else { - //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); //get left bound btAABB bound; bound.invalidate(); @@ -288,7 +287,7 @@ void btGImpactQuantizedBvh::refit() void btGImpactQuantizedBvh::buildSet() { //obtain primitive boxes - BT_BVH_DATA_ARRAY primitive_boxes; + GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); for (int i = 0;i<primitive_boxes.size() ;i++ ) diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h index 3a4774b849d..e9cccac75f5 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -29,12 +29,6 @@ subject to the following restrictions: -/*! \defgroup BOX_TREES - - - -*/ -//! @{ ///btQuantizedBvhNode is a compressed aabb node, 16 bytes. @@ -100,7 +94,7 @@ ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE -class BT_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE> +class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE> { }; @@ -112,19 +106,19 @@ class btQuantizedBvhTree { protected: int m_num_nodes; - BT_QUANTIZED_BVH_NODE_ARRAY m_node_array; + GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array; btAABB m_global_bound; btVector3 m_bvhQuantization; protected: - void calc_quantization(BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); + void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); int _sort_and_calc_splitting_index( - BT_BVH_DATA_ARRAY & primitive_boxes, + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(BT_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() { @@ -133,7 +127,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(BT_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 @@ -365,7 +359,6 @@ public: return m_box_tree.get_node_pointer(index); } -//! @} static float getAverageTreeCollisionTime(); diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h index 43f32bea5a1..05431cfce15 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h @@ -105,6 +105,7 @@ protected: public: btGImpactShapeInterface() { + m_shapeType=GIMPACT_SHAPE_PROXYTYPE; m_localAABB.invalidate(); m_needs_update = true; localScaling.setValue(1.f,1.f,1.f); @@ -880,6 +881,8 @@ Set of btGImpactMeshShapePart parts */ class btGImpactMeshShape : public btGImpactShapeInterface { + btStridingMeshInterface* m_meshInterface; + protected: btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts; void buildMeshParts(btStridingMeshInterface * meshInterface) @@ -906,6 +909,7 @@ protected: public: btGImpactMeshShape(btStridingMeshInterface * meshInterface) { + m_meshInterface = meshInterface; buildMeshParts(meshInterface); } @@ -921,6 +925,15 @@ public: } + btStridingMeshInterface* getMeshInterface() + { + return m_meshInterface; + } + + const btStridingMeshInterface* getMeshInterface() const + { + return m_meshInterface; + } int getMeshPartCount() const { @@ -1032,12 +1045,12 @@ public: } //! call when reading child shapes - virtual void lockChildShapes() + virtual void lockChildShapes() const { btAssert(0); } - virtual void unlockChildShapes() + virtual void unlockChildShapes() const { btAssert(0); } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp index 4b4b5e20dd3..956fa0430e3 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -251,16 +251,16 @@ bool btGenericPoolAllocator::freeMemory(void * pointer) #define BT_DEFAULT_POOL_ELEMENT_SIZE 8 // main allocator -class BT_STANDARD_ALLOCATOR: public btGenericPoolAllocator +class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator { public: - BT_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) { } }; // global allocator -BT_STANDARD_ALLOCATOR g_main_allocator; +GIM_STANDARD_ALLOCATOR g_main_allocator; void * btPoolAlloc(size_t size) diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h index 05b79a82516..e883bcd23d2 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h @@ -155,14 +155,9 @@ public: -/*! \defgroup POOL_MEMORY_FUNCTIONS -standar managed Memory functions. Memory pools are used. -*/ -//! @{ void * btPoolAlloc(size_t size); void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize); void btPoolFree(void *ptr); -//! @} #endif diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h index f5800716a96..bc5a416ddb7 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h @@ -29,9 +29,6 @@ subject to the following restrictions: -/*! \defgroup GEOMETRIC_OPERATIONS -*/ -//! @{ #define PLANEDIREPSILON 0.0000001f @@ -210,7 +207,6 @@ SIMD_FORCE_INLINE void bt_segment_collision( -//! @} #endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h index e9e6ad3d805..7faada61c95 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h @@ -29,9 +29,6 @@ subject to the following restrictions: -/*! \defgroup GEOMETRIC_OPERATIONS -*/ -//! @{ @@ -86,7 +83,6 @@ SIMD_FORCE_INLINE btVector3 bt_unquantize( return vecOut; } -//! @} #endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp index 0267f7e67c7..78696277c34 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: -void BT_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, +void GIM_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, btScalar margin, const btVector3 * points, int point_count) { m_point_count = 0; @@ -123,7 +123,7 @@ int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * return clipped_count; } -bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts) +bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts) { btScalar margin = m_margin + other.m_margin; @@ -132,7 +132,7 @@ bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangl //create planes // plane v vs U points - BT_TRIANGLE_CONTACT contacts1; + GIM_TRIANGLE_CONTACT contacts1; contacts1.m_separating_normal = m_plane; @@ -152,7 +152,7 @@ bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangl //Clip tri1 by tri2 edges - BT_TRIANGLE_CONTACT contacts2; + GIM_TRIANGLE_CONTACT contacts2; contacts2.m_separating_normal = other.m_plane; clipped_count = other.clip_triangle(*this,clipped_points); diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h index bdaa323d564..bbd6b630c02 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -35,14 +35,14 @@ subject to the following restrictions: #define MAX_TRI_CLIPPING 16 //! Structure for collision -struct BT_TRIANGLE_CONTACT +struct GIM_TRIANGLE_CONTACT { btScalar m_penetration_depth; int m_point_count; btVector4 m_separating_normal; btVector3 m_points[MAX_TRI_CLIPPING]; - SIMD_FORCE_INLINE void copy_from(const BT_TRIANGLE_CONTACT& other) + SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other) { m_penetration_depth = other.m_penetration_depth; m_separating_normal = other.m_separating_normal; @@ -54,11 +54,11 @@ struct BT_TRIANGLE_CONTACT } } - BT_TRIANGLE_CONTACT() + GIM_TRIANGLE_CONTACT() { } - BT_TRIANGLE_CONTACT(const BT_TRIANGLE_CONTACT& other) + GIM_TRIANGLE_CONTACT(const GIM_TRIANGLE_CONTACT& other) { copy_from(other); } @@ -123,7 +123,7 @@ public: /*! \pre this triangle and other must have their triangles calculated */ - bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts); + bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts); }; diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h index 89fbb5ab0a5..c8161d252d5 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h @@ -34,11 +34,6 @@ email: projectileman@yahoo.com #include "gim_memory.h" -/*! \addtogroup CONTAINERS -\brief -Abstract class for template containers -*/ -//! @{ #define GIM_ARRAY_GROW_INCREMENT 2 #define GIM_ARRAY_GROW_FACTOR 2 @@ -321,11 +316,10 @@ public: { resizeData(m_size); } -//!@} + }; -//! @} 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 d4cbce04b5e..666abf7917a 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -40,9 +40,6 @@ email: projectileman@yahoo.com -/*! \defgroup GEOMETRIC_OPERATIONS -*/ -//! @{ #define PLANEDIREPSILON 0.0000001f @@ -541,7 +538,6 @@ SIMD_FORCE_INLINE void SORT_3_INDICES( -//! @} #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 7784aeff140..322004a8d5b 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h @@ -34,11 +34,6 @@ email: projectileman@yahoo.com #include "gim_array.h" -/*! \addtogroup CONTAINERS -\brief -Abstract class for template containers -*/ -//! @{ #define GUINT_BIT_COUNT 32 #define GUINT_EXPONENT 5 @@ -122,7 +117,6 @@ public: }; -//! @} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h index c9bb83e3653..0add5e4b99f 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h @@ -35,9 +35,6 @@ email: projectileman@yahoo.com #include "gim_basic_geometry_operations.h" #include "LinearMath/btTransform.h" -/*! \defgroup BOUND_AABB_OPERATIONS -*/ -//! @{ //SIMD_FORCE_INLINE bool test_cross_edge_box( @@ -589,6 +586,5 @@ SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btT } -//! @} #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 1ffc2bbad78..0c3d7ba8db0 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp @@ -110,8 +110,7 @@ GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index( splitIndex = startIndex+ (numIndices>>1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - btAssert(!unbal); + btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); return splitIndex; } @@ -180,3 +179,4 @@ void GIM_BOX_TREE::build_tree( _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 ece5936de6b..1058a0872a5 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h @@ -40,12 +40,6 @@ email: projectileman@yahoo.com #include "gim_tri_collision.h" -/*! \defgroup BOX_PRUNNING - - - -*/ -//! @{ //! Overlapping pair struct GIM_PAIR @@ -446,7 +440,6 @@ public: m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle); } -//! @} }; //! Class for Box Tree Sets diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h index 62151cb97b7..a91fd3aa422 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h @@ -33,9 +33,6 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ -/*! \addtogroup GEOMETRIC_OPERATIONS -*/ -//! @{ //! This function calcs the distance from a 3D plane class DISTANCE_PLANE_3D_FUNC @@ -209,6 +206,5 @@ SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D( } -//! @} #endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h index 62df51b782f..ad3e12298fe 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h @@ -36,17 +36,6 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" #include "gim_array.h" -/*! \defgroup CONTACTS -\brief -Functions for managing and sorting contacts resulting from a collision query. -<ul> -<li> Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST -<li> After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY -<li> Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts -</ul> - -*/ -//! @{ /** Configuration var for applying interpolation of contact normals @@ -55,6 +44,10 @@ Configuration var for applying interpolation of contact normals #define CONTACT_DIFF_EPSILON 0.00001f /// Structure for collision results +///Functions for managing and sorting contacts resulting from a collision query. +///Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST +///After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY +///Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts class GIM_CONTACT { public: @@ -168,5 +161,4 @@ public: void merge_contacts_unique(const gim_contact_array & contacts); }; -//! @} #endif // GIM_CONTACT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h index cd3e53dcc0c..2fb1e3f9e45 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h @@ -36,11 +36,7 @@ email: projectileman@yahoo.com #include "gim_math.h" -/*! \defgroup GEOMETRIC_TYPES -\brief -Basic types and constants for geometry -*/ -//! @{ + //! Short Integer vector 2D typedef GSHORT vec2s[2]; @@ -95,7 +91,6 @@ typedef GREAL mat4f[4][4]; typedef GREAL quatf[4]; //typedef struct _aabb3f aabb3f; -//! @} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h index fd980dff5fa..93c66f81823 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h @@ -34,11 +34,6 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" -/*! \addtogroup CONTAINERS -\brief -Abstract class for collision objects -*/ -//! @{ #define GIM_INVALID_HASH 0xffffffff //!< A very very high value #define GIM_DEFAULT_HASH_TABLE_SIZE 380 @@ -204,12 +199,7 @@ protected: //SuperBufferedArray< _node_type > m_nodes; bool m_sorted; - /*! \defgroup HASH_TABLE_STRUCTURES - \brief - Hash table data management. The hash table has the indices to the corresponding m_nodes array - */ - //! @{ - + ///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;//!< @@ -459,13 +449,8 @@ protected: } - //! @} - - /*! \defgroup SORTED_ARRAY_STRUCTURES - \brief - Sorted array data management. The hash table has the indices to the corresponding m_nodes array - */ - //! @{ + + ///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; @@ -575,8 +560,7 @@ protected: return GIM_INVALID_HASH; } - //! @} - + public: @@ -913,8 +897,6 @@ public: }; -//! @} - #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 adf1c1c2ddf..0247d4e61cc 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h @@ -40,11 +40,6 @@ email: projectileman@yahoo.com -/*! \defgroup VECTOR_OPERATIONS -T -Operations for vectors : vec2f,vec3f and vec4f -*/ -//! @{ //! Zero out a 2D vector #define VEC_ZERO_2(a) \ @@ -446,13 +441,8 @@ Takes two vectors a, b, blends them together with s <=1 */ -//! @} -/*! \defgroup MATRIX_OPERATIONS -Operations for matrices : mat2f, mat3f and mat4f -*/ -//! @{ /// initialize matrix #define IDENTIFY_MATRIX_3X3(m) \ @@ -1579,6 +1569,5 @@ and m is a mat4f<br> }\ -//! @} #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 25ad1e21c7f..8b9e6806ef7 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h @@ -35,13 +35,6 @@ email: projectileman@yahoo.com #include "LinearMath/btScalar.h" -/*! \defgroup BASIC_TYPES -Basic types and constants -Conventions: -Types starting with G -Constants starting with G_ -*/ -//! @{ #define GREAL btScalar #define GREAL2 double @@ -52,15 +45,7 @@ Constants starting with G_ #define GINT64 long long #define GUINT64 unsigned long long -//! @} -/*! \defgroup BASIC_CONSTANTS -Basic constants -Conventions: -Types starting with G -Constants starting with G_ -*/ -//! @{ #define G_PI 3.14159265358979f #define G_HALF_PI 1.5707963f @@ -73,14 +58,9 @@ Constants starting with G_ #define G_REAL_INFINITY FLT_MAX #define G_SIGN_BITMASK 0x80000000 #define G_EPSILON SIMD_EPSILON -//! @} -/*! \defgroup SCALAR_TYPES -\brief -Precision type constants -*/ -//! @{ + enum GIM_SCALAR_TYPES { G_STYPE_REAL =0, @@ -92,12 +72,8 @@ enum GIM_SCALAR_TYPES G_STYPE_INT64, G_STYPE_UINT64 }; -//! @} -/*! \defgroup MATH_FUNCTIONS -mathematical functions -*/ -//! @{ + #define G_DEGTORAD(X) ((X)*3.1415926f/180.0f) #define G_RADTODEG(X) ((X)*180.0f/3.1415926f) @@ -131,7 +107,7 @@ mathematical functions ///returns a clamped number #define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number)) -#define GIM_GREATER(x, y) fabsf(x) > (y) +#define GIM_GREATER(x, y) btFabs(x) > (y) ///Swap numbers #define GIM_SWAP_NUMBERS(a,b){ \ @@ -176,6 +152,6 @@ inline GREAL gim_sqrt(GREAL f) return r; } -//! @} + #endif // GIM_MATH_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h index 469a8280f7d..fa99eebb60e 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h @@ -36,9 +36,6 @@ email: projectileman@yahoo.com #include "gim_math.h" #include <memory.h> -//#define PREFETCH 1 -//! \defgroup PREFETCH -//! @{ #ifdef PREFETCH #include <xmmintrin.h> // for prefetch #define pfval 64 @@ -53,13 +50,9 @@ email: projectileman@yahoo.com //! Prefetch 128 #define pf2(_x,_i) #endif -//! @} -/*! \defgroup ARRAY_UTILITIES -\brief -Functions for manip packed arrays of numbers -*/ -//! @{ + +///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_)\ @@ -92,50 +85,36 @@ Functions for manip packed arrays of numbers array[_i_] = constant;\ }\ }\ -//! @} -/*! \defgroup MEMORY_FUNCTION_PROTOTYPES -Function prototypes to allocate and free memory. -*/ -//! @{ + +///Function prototypes to allocate and free memory. typedef void * gim_alloc_function (size_t size); typedef void * gim_alloca_function (size_t size);//Allocs on the heap typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize); typedef void gim_free_function (void *ptr); -//! @} - -/*! \defgroup MEMORY_FUNCTION_HANDLERS -\brief -Memory Function Handlers - set new memory management functions. if fn is 0, the default handlers are - used. */ -//! @{ + + +///Memory Function Handlers +///set new memory management functions. if fn is 0, the default handlers are used. void gim_set_alloc_handler (gim_alloc_function *fn); void gim_set_alloca_handler (gim_alloca_function *fn); void gim_set_realloc_handler (gim_realloc_function *fn); void gim_set_free_handler (gim_free_function *fn); -//! @} -/*! \defgroup MEMORY_FUNCTION_GET_HANDLERS -\brief -get current memory management functions. -*/ -//! @{ + +///get current memory management functions. gim_alloc_function *gim_get_alloc_handler (void); gim_alloca_function *gim_get_alloca_handler(void); gim_realloc_function *gim_get_realloc_handler (void); gim_free_function *gim_get_free_handler (void); -//! @} -/*! \defgroup MEMORY_FUNCTIONS -Standar Memory functions -*/ -//! @{ + +///Standar Memory functions void * gim_alloc(size_t size); void * gim_alloca(size_t size); void * gim_realloc(void *ptr, size_t oldsize, size_t newsize); void gim_free(void *ptr); -//! @} + #if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h index 9ecf61bdc6d..f7dadbbca93 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h @@ -36,11 +36,7 @@ email: projectileman@yahoo.com #include "gim_memory.h" -/*! \defgroup SORTING -\brief -Macros for sorting. -*/ - +///Macros for sorting. //! Prototype for comparators class less_comparator { @@ -406,5 +402,5 @@ void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc) -//! @} + #endif // GIM_RADIXSORT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp index ef6feabdfee..74d734146a7 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -38,7 +38,7 @@ email: projectileman@yahoo.com #define MIN_EDGE_EDGE_DIS 0.00001f -class _GIM_TRIANGLE_CALCULATION_CACHE +class GIM_TRIANGLE_CALCULATION_CACHE { public: GREAL margin; @@ -489,7 +489,7 @@ public: }; -/*class _GIM_TRIANGLE_CALCULATION_CACHE +/*class GIM_TRIANGLE_CALCULATION_CACHE { public: GREAL margin; @@ -627,7 +627,7 @@ bool GIM_TRIANGLE::collide_triangle_hard_test( 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, diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h index 7cea8fc33a6..2d6e43a1a2e 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -36,9 +36,6 @@ email: projectileman@yahoo.com #include "gim_box_collision.h" #include "gim_clip_polygon.h" -/*! \addtogroup GEOMETRIC_OPERATIONS -*/ -//! @{ @@ -278,7 +275,7 @@ if 0.0<= u+v <=1.0 then they are inside of triangle } else { - float sumuv; + btScalar sumuv; sumuv = u+v; if(sumuv<-G_EPSILON) { @@ -378,6 +375,5 @@ if 0.0<= u+v <=1.0 then they are inside of triangle -//! @} #endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 6551cfb92fe..2f41b3c2757 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -135,6 +135,9 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //btScalar clippedDist = GEN_min(angularConservativeRadius,dist); //btScalar clippedDist = dist; + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + return false; dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); @@ -196,11 +199,10 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } } - - //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON) return false; - + result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 99690921317..412aace2114 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -21,7 +21,6 @@ class btStackAlloc; class btVector3; #include "btSimplexSolverInterface.h" class btConvexShape; -#include "LinearMath/btPoint3.h" class btTransform; ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. @@ -33,7 +32,7 @@ public: virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, + btVector3& v, btVector3& pa, btVector3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ) = 0; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index ccfc22ee673..ada20d3ef7a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -37,7 +37,7 @@ GJK-EPA collision solver by Nathanael Presson, 2008 namespace gjkepa2_impl { -// Config + // Config /* GJK */ #define GJK_MAX_ITERATIONS 128 @@ -58,711 +58,711 @@ namespace gjkepa2_impl #define EPA_INSIDE_EPS ((btScalar)0.01) -// Shorthands -typedef unsigned int U; -typedef unsigned char U1; + // Shorthands + typedef unsigned int U; + typedef unsigned char U1; -// MinkowskiDiff -struct MinkowskiDiff + // MinkowskiDiff + struct MinkowskiDiff { - const btConvexShape* m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; - btVector3 (btConvexShape::*Ls)(const btVector3&) const; - void EnableMargin(bool enable) + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + btVector3 (btConvexShape::*Ls)(const btVector3&) const; + void EnableMargin(bool enable) { - if(enable) - Ls=&btConvexShape::localGetSupportingVertex; + if(enable) + Ls=&btConvexShape::localGetSupportVertexNonVirtual; else - Ls=&btConvexShape::localGetSupportingVertexWithoutMargin; + Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; } - inline btVector3 Support0(const btVector3& d) const + inline btVector3 Support0(const btVector3& d) const { - return(((m_shapes[0])->*(Ls))(d)); + return(((m_shapes[0])->*(Ls))(d)); } - inline btVector3 Support1(const btVector3& d) const + inline btVector3 Support1(const btVector3& d) const { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); } - inline btVector3 Support(const btVector3& d) const + inline btVector3 Support(const btVector3& d) const { - return(Support0(d)-Support1(-d)); + return(Support0(d)-Support1(-d)); } - btVector3 Support(const btVector3& d,U index) const + btVector3 Support(const btVector3& d,U index) const { - if(index) - return(Support1(d)); + if(index) + return(Support1(d)); else - return(Support0(d)); + return(Support0(d)); } }; -typedef MinkowskiDiff tShape; + typedef MinkowskiDiff tShape; -// GJK -struct GJK -{ -/* Types */ -struct sSV - { - btVector3 d,w; - }; -struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; -struct eStatus { enum _ { - Valid, - Inside, - Failed };}; -/* Fields */ -tShape m_shape; -btVector3 m_ray; -btScalar m_distance; -sSimplex m_simplices[2]; -sSV m_store[4]; -sSV* m_free[4]; -U m_nfree; -U m_current; -sSimplex* m_simplex; -eStatus::_ m_status; -/* Methods */ - GJK() - { - Initialize(); - } -void Initialize() + // GJK + struct GJK { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } -eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rl<GJK_MIN_DISTANCE) - {/* Touching or inside */ - m_status=eStatus::Inside; - break; - } - /* Append new vertice in -'v' direction */ - appendvertice(cs,-m_ray); - const btVector3& w=cs.c[cs.rank-1]->w; - bool found=false; - for(U i=0;i<4;++i) + /* Types */ + struct sSV + { + btVector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + btVector3 m_ray; + btScalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + GJK() { - if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) - { found=true;break; } - } - if(found) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; + Initialize(); } - else - {/* Update lastw */ - lastw[clastw=(clastw+1)&3]=w; - } - /* Check for termination */ - const btScalar omega=dot(m_ray,w)/rl; - alpha=btMax(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - /* Reduce simplex */ - btScalar weights[4]; - U mask=0; - switch(cs.rank) + void Initialize() { - 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; + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; } - 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)) + eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + { + U iterations=0; + btScalar sqdist=0; + btScalar alpha=0; + btVector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rl<GJK_MIN_DISTANCE) + {/* Touching or inside */ + m_status=eStatus::Inside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs,-m_ray); + const btVector3& w=cs.c[cs.rank-1]->w; + bool found=false; + for(U i=0;i<4;++i) { - ns.c[ns.rank] = cs.c[i]; - ns.p[ns.rank++] = weights[i]; - m_ray += cs.c[i]->w*weights[i]; + if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) + { found=true;break; } + } + if(found) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; } else + {/* Update lastw */ + lastw[clastw=(clastw+1)&3]=w; + } + /* Check for termination */ + const btScalar omega=dot(m_ray,w)/rl; + alpha=btMax(omega,alpha); + if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask=0; + switch(cs.rank) { - m_free[m_nfree++] = cs.c[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(mask==15) m_status=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; - } while(m_status==eStatus::Valid); - m_simplex=&m_simplices[m_current]; - switch(m_status) - { - case eStatus::Valid: m_distance=m_ray.length();break; - case eStatus::Inside: m_distance=0;break; - } - return(m_status); - } -bool EncloseOrigin() - { - switch(m_simplex->rank) - { - case 1: - { - for(U i=0;i<3;++i) + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;i<ni;++i) + { + if(mask&(1<<i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; + } while(m_status==eStatus::Valid); + m_simplex=&m_simplices[m_current]; + switch(m_status) { - 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); - } + case eStatus::Valid: m_distance=m_ray.length();break; + case eStatus::Inside: m_distance=0;break; + } + return(m_status); } - break; - case 2: + bool EncloseOrigin() { - const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) + switch(m_simplex->rank) { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - const btVector3 p=cross(d,axis); - if(p.length2()>0) + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + const btVector3 p=cross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: { - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); + 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); } - break; - case 3: + /* Internals */ + void getsupport(const btVector3& d,sSV& sv) const { - const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - if(n.length2()>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); } - break; - case 4: + void removevertice(sSimplex& simplex) { - 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); + m_free[m_nfree++]=simplex.c[--simplex.rank]; } - break; - } - return(false); - } -/* Internals */ -void getsupport(const btVector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } -void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } -void appendvertice(sSimplex& simplex,const btVector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } -static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) - { - return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- - a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ - a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) - { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-dot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } - } - return(-1); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=cross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) - { - btScalar mindist=-1; - btScalar subw[2]; - U subm; - for(U i=0;i<3;++i) + 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) { - if(dot(*vt[i],cross(dl[i],n))>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()); + } + 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 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; - } + const btScalar t(l>0?-dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } } + return(-1); } - if(mindist<0) + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) { - const btScalar d=dot(a,n); - const btScalar s=btSqrt(l); - const btVector3 p=n*(d/l); - mindist = p.length2(); - m = 7; - w[0] = (cross(dl[1],b-p)).length()/s; - w[1] = (cross(dl[2],c-p)).length()/s; - w[2] = 1-(w[0]+w[1]); + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=cross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2]; + U subm; + for(U i=0;i<3;++i) + { + if(dot(*vt[i],cross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if(mindist<0) + { + const btScalar d=dot(a,n); + const btScalar s=btSqrt(l); + const btVector3 p=n*(d/l); + mindist = p.length2(); + m = 7; + w[0] = (cross(dl[1],b-p)).length()/s; + w[1] = (cross(dl[2],c-p)).length()/s; + w[2] = 1-(w[0]+w[1]); + } + return(mindist); + } + return(-1); } - return(mindist); - } - return(-1); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& d, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c,&d}; - const btVector3 dl[]={a-d,b-d,c-d}; - const btScalar vl=det(dl[0],dl[1],dl[2]); - const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0; - if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) - { - btScalar mindist=-1; - btScalar subw[3]; - U subm; - for(U i=0;i<3;++i) + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w,U& m) { - const U j=imd3[i]; - const btScalar s=vl*dot(d,cross(dl[i],dl[j])); - if(s>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*dot(a,cross(b-c,a-b)))<=0; + if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd<mindist)) + btScalar mindist=-1; + btScalar subw[3]; + U subm; + for(U i=0;i<3;++i) { - mindist = subd; - m = static_cast<U>((subm&1?1<<i:0)+ + const U j=imd3[i]; + const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + if(s>0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>((subm&1?1<<i:0)+ (subm&2?1<<j:0)+ (subm&4?8:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - w[3] = subw[2]; + 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); } - if(mindist<0) - { - mindist = 0; - m = 15; - w[0] = det(c,b,d)/vl; - w[1] = det(a,c,d)/vl; - w[2] = det(b,a,d)/vl; - w[3] = 1-(w[0]+w[1]+w[2]); - } - return(mindist); - } - return(-1); - } -}; - -// EPA -struct EPA -{ -/* Types */ -typedef GJK::sSV sSV; -struct sFace - { - btVector3 n; - btScalar d; - btScalar p; - sSV* c[3]; - sFace* f[3]; - sFace* l[2]; - U1 e[3]; - U1 pass; }; -struct sList - { - sFace* root; - U count; - sList() : root(0),count(0) {} - }; -struct sHorizon - { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(0),ff(0),nf(0) {} - }; -struct eStatus { enum _ { - Valid, - Touching, - Degenerated, - NonConvex, - InvalidHull, - OutOfFaces, - OutOfVertices, - AccuraryReached, - FallBack, - Failed };}; -/* Fields */ -eStatus::_ m_status; -GJK::sSimplex m_result; -btVector3 m_normal; -btScalar m_depth; -sSV m_sv_store[EPA_MAX_VERTICES]; -sFace m_fc_store[EPA_MAX_FACES]; -U m_nextsv; -sList m_hull; -sList m_stock; -/* Methods */ - EPA() - { - Initialize(); - } - - static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } -static inline void append(sList& list,sFace* face) + // EPA + struct EPA { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } -static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } - - -void Initialize() - { - m_status = eStatus::Failed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i<EPA_MAX_FACES;++i) + /* Types */ + typedef GJK::sSV sSV; + struct sFace { - 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()) + btVector3 n; + btScalar d; + btScalar p; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0),count(0) {} + }; + struct sHorizon { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0),ff(0),nf(0) {} + }; + struct eStatus { enum _ { + Valid, + Touching, + Degenerated, + NonConvex, + InvalidHull, + OutOfFaces, + OutOfVertices, + AccuraryReached, + FallBack, + Failed };}; + /* Fields */ + eStatus::_ m_status; + GJK::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } - /* Clean up */ - while(m_hull.root) + + static inline void bind(sFace* fa,U ea,sFace* fb,U eb) { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; } - 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) + static inline void append(sList& list,sFace* face) { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i<EPA_MAX_FACES;++i) + { + append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); + } + } + eStatus::_ Evaluate(GJK& gjk,const btVector3& guess) + { + GJK::sSimplex& simplex=*gjk.m_simplex; + if((simplex.rank>1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), newface(simplex.c[1],simplex.c[0],simplex.c[3],true), newface(simplex.c[2],simplex.c[1],simplex.c[3],true), newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterations<EPA_MAX_ITERATIONS;++iterations) - { - if(m_nextsv<EPA_MAX_VERTICES) - { - sHorizon horizon; - sSV* w=&m_sv_store[m_nextsv++]; - bool valid=true; - best->pass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=dot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterations<EPA_MAX_ITERATIONS;++iterations) { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, + if(m_nextsv<EPA_MAX_VERTICES) + { + sHorizon horizon; + sSV* w=&m_sv_store[m_nextsv++]; + bool valid=true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, best->f[j],best->e[j], horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - if(best->p>=outer.p) outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = cross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = cross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = cross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } -sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = cross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - face->p = btMin(btMin( - dot(a->w,cross(face->n,a->w-b->w)), - dot(b->w,cross(face->n,b->w-c->w))), - dot(c->w,cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) { - face->d = dot(a->w,face->n)/l; - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) + if(m_stock.root) { - return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; - remove(m_hull,face); - append(m_stock,face); - return(0); - } - m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - return(0); - } -sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - btScalar maxp=minf->p; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if((f->p>=maxp)&&(sqd<mind)) - { - minf=f; - mind=sqd; - maxp=f->p; + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = cross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = btMin(btMin( + dot(a->w,cross(face->n,a->w-b->w)), + dot(b->w,cross(face->n,b->w-c->w))), + dot(c->w,cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); } - } - return(minf); - } -bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) - { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) - { - const U e1=i1m3[e]; - if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + sFace* findbest() { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + btScalar maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) { - 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); + const btScalar sqd=f->d*f->d; + if((f->p>=maxp)&&(sqd<mind)) + { + minf=f; + mind=sqd; + maxp=f->p; + } } + return(minf); } - else + bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) { - 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)) + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) { - remove(m_hull,f); - append(m_stock,f); - return(true); + const U e1=i1m3[e]; + if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } } + return(false); } - } - 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); -} + // + 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); + } } @@ -775,87 +775,87 @@ using namespace gjkepa2_impl; // 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) + 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) + 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: { - 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); + 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; + break; case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; + results.status=sResults::GJK_Failed; + break; } -return(false); + return(false); } // @@ -865,49 +865,49 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, 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->getMargin()+ - shape1.getMargin(); - 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; + return(length-margin); } 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(); - if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-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); } } } -return(SIMD_INFINITY); + return(SIMD_INFINITY); } // @@ -918,10 +918,10 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, 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); } /* Symbols cleanup */ diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index c4f84ed4d75..05573c7cfce 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -17,13 +17,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" + + #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, - btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) { @@ -33,20 +34,12 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim const btScalar radialmargin(btScalar(0.)); -//#define USE_ORIGINAL_GJK 1 -#ifdef USE_ORIGINAL_GJK - btGjkEpaSolver::sResults results; - if(btGjkEpaSolver::Collide( pConvexA,transformA, - pConvexB,transformB, - radialmargin,stackAlloc,results)) -#else btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); btGjkEpaSolver2::sResults results; if(btGjkEpaSolver2::Penetration(pConvexA,transformA, pConvexB,transformB, guessVector,results)) -#endif { // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 2dc069ce5cf..68dbc566518 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -28,7 +28,7 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, - btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); private : diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 01fb1a4b068..0856332d1ca 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -18,11 +18,15 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" + + #if defined(DEBUG) || defined (_DEBUG) +//#define TEST_NON_VIRTUAL 1 #include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf +//#define DEBUG_SPU_COLLISION_DETECTION 1 #endif //__SPU__ #endif @@ -49,6 +53,8 @@ m_catchDegeneracies(1) void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) { + m_cachedSeparatingDistance = 0.f; + btScalar distance=btScalar(0.); btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 pointOnA,pointOnB; @@ -58,16 +64,35 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; +#ifdef __SPU__ + btScalar marginA = m_minkowskiA->getMarginNonVirtual(); + btScalar marginB = m_minkowskiB->getMarginNonVirtual(); +#else btScalar marginA = m_minkowskiA->getMargin(); btScalar marginB = m_minkowskiB->getMargin(); +#ifdef TEST_NON_VIRTUAL + btScalar marginAv = m_minkowskiA->getMarginNonVirtual(); + btScalar marginBv = m_minkowskiB->getMarginNonVirtual(); + btAssert(marginA == marginAv); + btAssert(marginB == marginBv); +#endif //TEST_NON_VIRTUAL +#endif + + gNumGjkChecks++; +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("inside gjk\n"); +#endif //for CCD we don't use margins if (m_ignoreMargin) { marginA = btScalar(0.); marginB = btScalar(0.); +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("ignoring margin\n"); +#endif } m_curIter = 0; @@ -98,18 +123,35 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); +#ifdef __SPU__ + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); +#else btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); - btPoint3 pWorld = localTransA(pInA); - btPoint3 qWorld = localTransB(qInB); - +#ifdef TEST_NON_VIRTUAL + btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); + btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + btAssert((pInAv-pInA).length() < 0.0001); + btAssert((qInBv-qInB).length() < 0.0001); +#endif // +#endif //__SPU__ + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("got local supporting vertices\n"); +#endif + 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)) { - checkPenetration = false; + checkSimplex=true; + //checkPenetration = false; break; } @@ -133,9 +175,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& checkSimplex = true; break; } + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("addVertex 1\n"); +#endif //add current vertex to simplex m_simplexSolver->addVertex(w, pWorld, qWorld); - +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("addVertex 2\n"); +#endif //calculate the closest point to the origin (update vector v) if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) { @@ -167,7 +215,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject if (m_curIter++ > gGjkMaxIter) { - #if defined(DEBUG) || defined (_DEBUG) + #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION) printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", @@ -290,10 +338,20 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& #endif //__CELLOS_LV2__ +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("output 1\n"); +#endif + m_cachedSeparatingAxis = normalInB; + m_cachedSeparatingDistance = distance; + output.addContactPoint( normalInB, pointOnB+positionOffset, distance); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("output 2\n"); +#endif //printf("gjk add:%f",distance); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 550fc4677e0..0ad4aab8a59 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -20,7 +20,6 @@ subject to the following restrictions: #define GJK_PAIR_DETECTOR_H #include "btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" class btConvexShape; @@ -38,6 +37,7 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; bool m_ignoreMargin; + btScalar m_cachedSeparatingDistance; public: @@ -68,6 +68,15 @@ public: m_cachedSeparatingAxis = seperatingAxis; } + const btVector3& getCachedSeparatingAxis() const + { + return m_cachedSeparatingAxis; + } + btScalar getCachedSeparatingDistance() const + { + return m_cachedSeparatingDistance; + } + void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) { m_penetrationDepthSolver = penetrationDepthSolver; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index e75fc1bee96..c7c9812985d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -32,6 +32,8 @@ class btManifoldPoint :m_userPersistentData(0), m_appliedImpulse(0.f), m_lateralFrictionInitialized(false), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), m_lifeTime(0) { } @@ -110,6 +112,12 @@ class btManifoldPoint m_distance1 = dist; } + ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver + btScalar getAppliedImpulse() const + { + return m_appliedImpulse; + } + }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 0e6fa2e6dfe..581b4258f03 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -19,9 +19,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" - - - #define NUM_UNITSPHERE_POINTS 42 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = { @@ -73,7 +70,7 @@ btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, + btVector3& v, btVector3& pa, btVector3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ) { @@ -117,7 +114,9 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 seperatingAxisInA,seperatingAxisInB; 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]; @@ -200,6 +199,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numSampleDirections = NUM_UNITSPHERE_POINTS; +#ifndef __SPU__ { int numPDA = convexA->getNumPreferredPenetrationDirections(); if (numPDA) @@ -229,14 +229,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s } } } +#endif // __SPU__ for (int i=0;i<numSampleDirections;i++) { const btVector3& norm = sPenetrationDirections[i]; seperatingAxisInA = (-norm)* transA.getBasis(); seperatingAxisInB = norm* transB.getBasis(); - pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); - qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; @@ -254,13 +255,13 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //add the margins - minA += minNorm*convexA->getMargin(); - minB -= minNorm*convexB->getMargin(); + minA += minNorm*convexA->getMarginNonVirtual(); + minB -= minNorm*convexB->getMarginNonVirtual(); //no penetration if (minProj < btScalar(0.)) return false; - minProj += (convexA->getMargin() + convexB->getMargin()); + minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index 27b42c2b47e..23cbd57ac7e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -27,10 +27,9 @@ public: virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, + btVector3& v, btVector3& pa, btVector3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); - }; #endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 386885d2ac8..d4e29882d37 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btPersistentManifold.h" #include "LinearMath/btTransform.h" -#include <assert.h> + btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; @@ -66,7 +66,7 @@ void btPersistentManifold::clearUserCache(btManifoldPoint& pt) printf("error in clearUserCache\n"); } } - assert(occurance<=0); + btAssert(occurance<=0); #endif //DEBUG_PERSISTENCY if (pt.m_userPersistentData && gContactDestroyedCallback) @@ -164,7 +164,7 @@ int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) { - assert(validContactDistance(newPoint)); + btAssert(validContactDistance(newPoint)); int insertIndex = getNumContacts(); if (insertIndex == MANIFOLD_CACHE_SIZE) @@ -183,6 +183,9 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) } + if (insertIndex<0) + insertIndex=0; + btAssert(m_pointCache[insertIndex].m_userPersistentData==0); m_pointCache[insertIndex] = newPoint; return insertIndex; @@ -190,7 +193,7 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) btScalar btPersistentManifold::getContactBreakingThreshold() const { - return gContactBreakingThreshold; + return m_contactBreakingThreshold; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index c122eb865e8..0b3c734d1d7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -24,7 +24,7 @@ subject to the following restrictions: struct btCollisionResult; -///contact breaking and merging threshold +///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); @@ -54,6 +54,9 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold void* m_body1; int m_cachedPoints; + btScalar m_contactBreakingThreshold; + btScalar m_contactProcessingThreshold; + /// sort cached points so most isolated points come first int sortCachedPoints(const btManifoldPoint& pt); @@ -68,10 +71,12 @@ public: btPersistentManifold(); - btPersistentManifold(void* body0,void* body1,int bla) - : m_body0(body0),m_body1(body1),m_cachedPoints(0) + btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) + : m_body0(body0),m_body1(body1),m_cachedPoints(0), + m_contactBreakingThreshold(contactBreakingThreshold), + m_contactProcessingThreshold(contactProcessingThreshold) { - (void)bla; + } SIMD_FORCE_INLINE void* getBody0() { return m_body0;} @@ -106,8 +111,13 @@ public: return m_pointCache[index]; } - /// todo: get this margin from the current physics / collision environment + ///@todo: get this margin from the current physics / collision environment btScalar getContactBreakingThreshold() const; + + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } int getCacheEntry(const btManifoldPoint& newPoint) const; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index a70a3ae56c4..cdb1d22444d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -23,10 +23,12 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "btRaycastCallback.h" -btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to) +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.)) { @@ -55,6 +57,12 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, { return ; // same sign } + //@BP Mod - Backface filtering + if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a > btScalar(0.0))) + { + // Backface, skip check + return; + } const btScalar proj_length=dist_a-dist_b; const btScalar distance = (dist_a)/(proj_length); @@ -89,14 +97,18 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) { + //@BP Mod + // Triangle normal isn't normalized + triangleNormal.normalize(); - if ( dist_a > 0 ) + //@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); } } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index d2b4b80f8ba..3a1ab388c13 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -29,9 +29,20 @@ public: 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 + + kF_Terminator = 0xFFFFFFFF + }; + unsigned int m_flags; + btScalar m_hitFraction; - btTriangleRaycastCallback(const btVector3& from,const btVector3& to); + btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index cf65f46505b..823b4e7158b 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -19,7 +19,6 @@ subject to the following restrictions: #define SIMPLEX_SOLVER_INTERFACE_H #include "LinearMath/btVector3.h" -#include "LinearMath/btPoint3.h" #define NO_VIRTUAL_INTERFACE 1 #ifdef NO_VIRTUAL_INTERFACE @@ -37,7 +36,7 @@ class btSimplexSolverInterface virtual void reset() = 0; - virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) = 0; + virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0; virtual bool closest(btVector3& v) = 0; @@ -45,7 +44,7 @@ class btSimplexSolverInterface virtual bool fullSimplex() const = 0; - virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0; + virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0; virtual bool inSimplex(const btVector3& w) = 0; @@ -53,7 +52,7 @@ class btSimplexSolverInterface virtual bool emptySimplex() const = 0; - virtual void compute_points(btPoint3& p1, btPoint3& p2) = 0; + virtual void compute_points(btVector3& p1, btVector3& p2) = 0; virtual int numVertices() const =0; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index cf8a3ab5eb1..a7ffeda8c62 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -25,8 +25,6 @@ subject to the following restrictions: #include "btVoronoiSimplexSolver.h" -#include <assert.h> -//#include <stdio.h> #define VERTA 0 #define VERTB 1 @@ -37,7 +35,7 @@ subject to the following restrictions: void btVoronoiSimplexSolver::removeVertex(int index) { - assert(m_numVertices>0); + btAssert(m_numVertices>0); m_numVertices--; m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; @@ -77,7 +75,7 @@ void btVoronoiSimplexSolver::reset() //add a vertex -void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) +void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) { m_lastW = w; m_needsUpdate = true; @@ -267,7 +265,7 @@ btScalar btVoronoiSimplexSolver::maxVertex() //return the current simplex -int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const +int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const { int i; for (i=0;i<numVertices();i++) @@ -314,7 +312,7 @@ bool btVoronoiSimplexSolver::emptySimplex() const } -void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) +void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) { updateClosestVectorAndPoints(); p1 = m_cachedP1; @@ -325,7 +323,7 @@ void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) -bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result) +bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result) { result.m_usedVertices.reset(); @@ -425,7 +423,7 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP /// Test if point p and d lie on opposite sides of plane through abc -int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d) +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); @@ -452,7 +450,7 @@ if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) } -bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& 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; @@ -486,7 +484,7 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const if (pointOutsideABC) { closestPtPointTriangle(p, a, b, c,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; + btVector3 q = tempResult.m_closestPointOnSimplex; btScalar sqDist = (q - p).dot( q - p); // Update best closest point if (squared) distance is less than current best @@ -513,7 +511,7 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const if (pointOutsideACD) { closestPtPointTriangle(p, a, c, d,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; + btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! btScalar sqDist = (q - p).dot( q - p); @@ -541,7 +539,7 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const if (pointOutsideADB) { closestPtPointTriangle(p, a, d, b,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; + btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! btScalar sqDist = (q - p).dot( q - p); @@ -569,7 +567,7 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const if (pointOutsideBDC) { closestPtPointTriangle(p, b, d, c,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; + btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 356d335bc93..d3162d9fbeb 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -50,7 +50,7 @@ struct btUsageBitfield{ struct btSubSimplexClosestResult { - btPoint3 m_closestPointOnSimplex; + btVector3 m_closestPointOnSimplex; //MASK for m_usedVertices //stores the simplex vertex-usage, using the MASK, // if m_usedVertices & MASK then the related vertex is used @@ -97,13 +97,13 @@ public: int m_numVertices; btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; - btPoint3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; - btPoint3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; - btPoint3 m_cachedP1; - btPoint3 m_cachedP2; + btVector3 m_cachedP1; + btVector3 m_cachedP2; btVector3 m_cachedV; btVector3 m_lastW; bool m_cachedValidClosest; @@ -116,15 +116,15 @@ public: void reduceVertices (const btUsageBitfield& usedVerts); bool updateClosestVectorAndPoints(); - bool closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult); - int pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d); - bool closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result); + 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: void reset(); - void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q); + void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); bool closest(btVector3& v); @@ -136,7 +136,7 @@ public: return (m_numVertices == 4); } - int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const; + int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; bool inSimplex(const btVector3& w); @@ -144,7 +144,7 @@ public: bool emptySimplex() const ; - void compute_points(btPoint3& p1, btPoint3& p2) ; + void compute_points(btVector3& p1, btVector3& p2) ; int numVertices() const { |