diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp index 629cc67d9..5006f9d64 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp @@ -94,17 +94,20 @@ throw(object); } #else template static inline void Raise(const T&) {} #endif + + struct Block + { + Block* previous; + U1* address; + }; // // StackAlloc // struct StackAlloc - { - struct Block - { - Block* previous; - U1* address; - }; + { + + StackAlloc() { ctor(); } StackAlloc(U size) { ctor();Create(size); } ~StackAlloc() { Free(); } @@ -122,10 +125,21 @@ struct StackAlloc data = 0; usedsize = 0; } else Raise(L"StackAlloc is still in use"); + } + U1* Allocate(U size) + { + const U nus(usedsize+size); + if(nusprevious = current; pb->address = data+usedsize; current = pb; @@ -139,17 +153,7 @@ struct StackAlloc usedsize = (U)((block->address-data)-sizeof(Block)); } else Raise(L"Unmatched blocks"); } - U1* Allocate(U size) - { - const U nus(usedsize+size); - if(nusv==ray) { --order;return(false); } else e=e->n; } e=(He*)sa->Allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e; Support(ray,simplex[++order]); @@ -426,10 +430,10 @@ struct EPA // inline Face* FindBest() const { - Face* bf(0); + Face* bf = 0; if(root) { - Face* cf(root); + Face* cf = root; F bd(cstInf); do { if(cf->dd;bf=cf; } @@ -457,7 +461,7 @@ c) const // inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) { - Face* pf((Face*)sa->Allocate(sizeof(Face))); + Face* pf = (Face*)sa->Allocate(sizeof(Face)); if(Set(pf,a,b,c)) { if(root) root->prev=pf; @@ -499,7 +503,7 @@ c) const // GJK::Mkv* Support(const Vector3& w) const { - GJK::Mkv* v((GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv))); + GJK::Mkv* v =(GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); gjk->Support(w,*v); return(v); } @@ -514,7 +518,7 @@ ff) const U e1(mod3[e+1]); if((f.n.dot(w->w)+f.d)>0) { - Face* nf(NewFace(f.v[e1],f.v[e],w)); + Face* nf = NewFace(f.v[e1],f.v[e],w); Link(nf,0,&f,e); if(cf) Link(cf,1,nf,2); else ff=nf; cf=nf;ne=1; @@ -533,8 +537,8 @@ ff) // inline F EvaluatePD(F accuracy=EPA_accuracy) { - StackAlloc::Block* sablock(sa->BeginBlock()); - Face* bestface(0); + Block* sablock = sa->BeginBlock(); + Face* bestface = 0; U markid(1); depth = -cstInf; normal = Vector3(0,0,0); @@ -545,10 +549,10 @@ ff) /* Prepare hull */ if(gjk->EncloseOrigin()) { - const U* pfidx(0); - U nfidx(0); - const U* peidx(0); - U neidx(0); + const U* pfidx = 0; + U nfidx= 0; + const U* peidx = 0; + U neidx = 0; GJK::Mkv* basemkv[5]; Face* basefaces[6]; switch(gjk->order) @@ -568,14 +572,16 @@ U fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; U eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}}; pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9; } break; - } - for(U i=0;i<=gjk->order;++i) { + } + U i; + + for( i=0;i<=gjk->order;++i) { basemkv[i]=(GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; } - for(U i=0;in)); + GJK::Mkv* w = Support(-bf->n); const F d(bf->n.dot(w->w)+bf->d); bestface = bf; if(d<-accuracy) { - Face* cf(0); - Face* ff(0); - U nf(0); + Face* cf =0; + Face* ff =0; + U nf = 0; Detach(bf); bf->mark=++markid; for(U i=0;i<3;++i) { diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index cf33b1d67..ce85fc398 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -47,10 +47,47 @@ unsigned long btRand2() return btSeed2; } + + + +//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) int btRandInt2 (int n) +{ + // seems good; xor-fold and modulus + const unsigned long un = n; + unsigned long r = btRand2(); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000UL) { + r ^= (r >> 16); + if (un <= 0x00000100UL) { + r ^= (r >> 8); + if (un <= 0x00000010UL) { + r ^= (r >> 4); + if (un <= 0x00000004UL) { + r ^= (r >> 2); + if (un <= 0x00000002UL) { + r ^= (r >> 1); + } + } + } + } + } + + return (int) (r % un); +} + + + +int btRandIntWrong (int n) { float a = float(n) / 4294967296.0f; - return (int) (float(btRand2()) * a); +// printf("n = %d\n",n); +// printf("a = %f\n",a); + int res = (int) (float(btRand2()) * a); +// printf("res=%d\n",res); + return res; } bool MyContactDestroyedCallback(void* userPersistentData) @@ -63,14 +100,14 @@ bool MyContactDestroyedCallback(void* userPersistentData) return true; } -btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2() +btSequentialImpulseConstraintSolver3::btSequentialImpulseConstraintSolver3() { - setSolverMode(SOLVER_USE_WARMSTARTING); + setSolverMode(SOLVER_RANDMIZE_ORDER); } btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_solverMode(SOLVER_RANDMIZE_ORDER) +:m_solverMode(SOLVER_USE_WARMSTARTING) { gContactDestroyedCallback = &MyContactDestroyedCallback; @@ -86,7 +123,7 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() } /// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { btContactSolverInfo info = infoGlobal; @@ -105,6 +142,7 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man { btPersistentManifold* manifold = manifoldPtr[j]; prepareConstraints(manifold,info,debugDrawer); + for (int p=0;pgetNumContacts();p++) { gOrder[totalPoints].m_manifoldIndex = j; @@ -159,7 +197,7 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man /// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { btContactSolverInfo info = infoGlobal; @@ -292,7 +330,9 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } else { - cpd = new btConstraintPersistentData(); + cpd = new btConstraintPersistentData; + assert(cpd); + totalCpd ++; //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); cp.m_userPersistentData = cpd; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index bcc3806b9..ec56af14d 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -82,11 +82,11 @@ public: }; /// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering -class btSequentialImpulseConstraintSolver2 : public btSequentialImpulseConstraintSolver +class btSequentialImpulseConstraintSolver3 : public btSequentialImpulseConstraintSolver { public: - btSequentialImpulseConstraintSolver2(); + btSequentialImpulseConstraintSolver3(); virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);