diff options
| author | Erwin Coumans <blender@erwincoumans.com> | 2009-03-03 06:41:24 +0300 |
|---|---|---|
| committer | Erwin Coumans <blender@erwincoumans.com> | 2009-03-03 06:41:24 +0300 |
| commit | 982a5cc60dd93b21ce110b3fff831d05720047bf (patch) | |
| tree | 5d6245b0006f249c48e900b748aedd4484c10da3 /extern/bullet2/src/BulletCollision/BroadphaseCollision | |
| parent | f8ef887880ac217cc831b2ab9677e1f9e825537b (diff) | |
Upgraded to Bullet 2.74. The upgrade introduced a few bugs, which need to be fixed before Blender 2.49.
In particular, the Bullet vehicle seems broken, and some soft-body demos don't work.
No new features or benefits are added yet, but a few improvements are planned before Blender 2.49 release.
Please update the build systems, and add those 3 files:
extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
I'll watch the Blender mailing list, in case this commit causes some issues.
Diffstat (limited to 'extern/bullet2/src/BulletCollision/BroadphaseCollision')
17 files changed, 2609 insertions, 1848 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() { |