diff --git a/Extras/quickstep/btSorLcp.cpp b/Extras/quickstep/btSorLcp.cpp index 23698fa0e..d4d87393e 100644 --- a/Extras/quickstep/btSorLcp.cpp +++ b/Extras/quickstep/btSorLcp.cpp @@ -18,6 +18,7 @@ subject to the following restrictions: #include "btSorLcp.h" #include "btOdeSolverBody.h" +#include "LinearMath/btQuickProf.h" #ifdef USE_SOR_SOLVER @@ -223,6 +224,8 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, btStackAlloc* stackAlloc ) { + BT_PROFILE("btSorLcpSolver::SOR_LCP"); + //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 AutoBlockSa asaBlock(stackAlloc); @@ -448,6 +451,7 @@ void btSorLcpSolver::SolveInternal1 ( int nj, const btContactSolverInfo& solverInfo, btStackAlloc* stackAlloc) { + BT_PROFILE("btSorLcpSolver::SolveInternal1"); //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 AutoBlockSa asaBlock(stackAlloc); diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index b74cc19fa..297298d5e 100644 --- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -138,6 +138,8 @@ public: 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;} + void resetPool(); + void processAllOverlappingPairs(btOverlapCallback* callback); //Broadphase Interface @@ -584,6 +586,21 @@ void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,bt } +template +void btAxisSweep3Internal::resetPool() +{ + 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(i + 1)); + m_pHandles[m_maxHandles - 1].SetNextFree(0); + } + } +} + + extern int gOverlappingPairs; //#include diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index 676707a3b..88d1d293a 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -184,7 +184,8 @@ struct btDbvtNode btDbvtNode* parent; DBVT_INLINE bool isleaf() const { return(childs[1]==0); } DBVT_INLINE bool isinternal() const { return(!isleaf()); } - union { + union + { btDbvtNode* childs[2]; void* data; int dataAsInt; @@ -264,8 +265,8 @@ struct btDbvt int m_leaves; unsigned m_opath; - mutable btAlignedObjectArray m_stack; - mutable btAlignedObjectArray m_stkStack; + + // Methods @@ -727,22 +728,23 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, { int depth=1; int treshold=DOUBLE_STACKSIZE-4; - m_stkStack.resize(DOUBLE_STACKSIZE); - m_stkStack[0]=sStkNN(root0,root1); + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); do { - sStkNN p=m_stkStack[--depth]; + sStkNN p=stkStack[--depth]; if(depth>treshold) { - m_stkStack.resize(m_stkStack.size()*2); - treshold=m_stkStack.size()-4; + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; } if(p.a==p.b) { 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]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); } } else if(Intersect(p.a->volume,p.b->volume)) @@ -751,23 +753,23 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, { 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]); + 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 { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); } } else { if(p.b->isinternal()) { - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); } else { @@ -791,38 +793,38 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, { int depth=1; int treshold=DOUBLE_STACKSIZE-4; - m_stkStack.resize(DOUBLE_STACKSIZE); - m_stkStack[0]=sStkNN(root0,root1); + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); do { - sStkNN p=m_stkStack[--depth]; + sStkNN p=stkStack[--depth]; if(Intersect(p.a->volume,p.b->volume,xform)) { if(depth>treshold) { - m_stkStack.resize(m_stkStack.size()*2); - treshold=m_stkStack.size()-4; + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; } 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]); + 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 { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); } } else { if(p.b->isinternal()) { - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); } else { @@ -856,26 +858,26 @@ inline void btDbvt::collideTV( const btDbvtNode* root, if(root) { ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - - m_stack.resize(0); - m_stack.reserve(SIMPLE_STACKSIZE); - m_stack.push_back(root); + btAlignedObjectArray stack; + stack.resize(0); + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); do { - const btDbvtNode* n=m_stack[m_stack.size()-1]; - m_stack.pop_back(); + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); if(Intersect(n->volume,volume)) { if(n->isinternal()) { - m_stack.push_back(n->childs[0]); - m_stack.push_back(n->childs[1]); + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); } else { policy.Process(n); } } - } while(m_stack.size()>0); + } while(stack.size()>0); } } @@ -895,13 +897,13 @@ inline void btDbvt::rayTestInternal( const btDbvtNode* root, int depth=1; int treshold=DOUBLE_STACKSIZE-2; - - m_stack.resize(DOUBLE_STACKSIZE); - m_stack[0]=root; + btAlignedObjectArray stack; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; btVector3 bounds[2]; do { - const btDbvtNode* node=m_stack[--depth]; + const btDbvtNode* node=stack[--depth]; bounds[0] = node->volume.Mins(); bounds[1] = node->volume.Maxs(); btScalar tmin=1.f,lambda_min=0.f; @@ -913,11 +915,11 @@ inline void btDbvt::rayTestInternal( const btDbvtNode* root, { if(depth>treshold) { - m_stack.resize(m_stack.size()*2); - treshold=m_stack.size()-2; + stack.resize(stack.size()*2); + treshold=stack.size()-2; } - m_stack[depth++]=node->childs[0]; - m_stack[depth++]=node->childs[1]; + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; } else { diff --git a/src/BulletCollision/CMakeLists.txt b/src/BulletCollision/CMakeLists.txt index 4837b7e88..331061d7d 100644 --- a/src/BulletCollision/CMakeLists.txt +++ b/src/BulletCollision/CMakeLists.txt @@ -143,8 +143,6 @@ ADD_LIBRARY(LibBulletCollision Gimpact/gim_box_set.cpp NarrowPhaseCollision/btContinuousConvexCollision.cpp NarrowPhaseCollision/btContinuousConvexCollision.h - NarrowPhaseCollision/btGjkEpa.cpp - NarrowPhaseCollision/btGjkEpa.h NarrowPhaseCollision/btGjkEpa2.cpp NarrowPhaseCollision/btGjkEpa2.h NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp diff --git a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index fbfc17290..83dcb5c6f 100644 --- a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -207,7 +207,13 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) cy += q*(p[i*2+1]+p[i*2+3]); } q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - a = 1.f/(btScalar(3.0)*(a+q)); + if (btFabs(a+q) > SIMD_EPSILON) + { + a = 1.f/(btScalar(3.0)*(a+q)); + } else + { + a=1e30f; + } cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); } diff --git a/src/BulletCollision/Gimpact/btBoxCollision.h b/src/BulletCollision/Gimpact/btBoxCollision.h index f42ed0f6d..827a3c895 100644 --- a/src/BulletCollision/Gimpact/btBoxCollision.h +++ b/src/BulletCollision/Gimpact/btBoxCollision.h @@ -38,7 +38,7 @@ subject to the following restrictions: #define BT_MAX(a,b) (ab?b:a) -#define BT_GREATER(x, y) fabsf(x) > (y) +#define BT_GREATER(x, y) btFabs(x) > (y) #define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c)) #define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c)) @@ -178,7 +178,7 @@ public: { for(j=0;j<3;j++ ) { - m_AR[i][j] = 1e-6f + fabsf(m_R1to0[i][j]); + m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); } } diff --git a/src/BulletCollision/ibmsdk/Makefile b/src/BulletCollision/ibmsdk/Makefile index 2fc66c095..ff3a6bf95 100644 --- a/src/BulletCollision/ibmsdk/Makefile +++ b/src/BulletCollision/ibmsdk/Makefile @@ -39,7 +39,6 @@ btSimpleBroadphase.o \ btContinuousConvexCollision.o \ btConvexCast.o \ btGjkConvexCast.o \ -btGjkEpa.o \ btGjkEpa2.o \ btGjkEpaPenetrationDepthSolver.o \ btGjkPairDetector.o \ diff --git a/src/Makefile.am b/src/Makefile.am index 3bda6d975..d7fd17e13 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -33,7 +33,6 @@ libbulletcollision_a_SOURCES = \ BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp \ BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp \ BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp \ - BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp \ BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp \ BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp \ BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp \ @@ -115,7 +114,6 @@ libbulletcollision_a_SOURCES = \ BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h \ BulletCollision/NarrowPhaseCollision/btRaycastCallback.h \ BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h \ - BulletCollision/NarrowPhaseCollision/btGjkEpa.h \ BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h \ BulletCollision/NarrowPhaseCollision/btPersistentManifold.h \ BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h \