From d58670ddd2e43c9ba6e56c766827165eb3ba08cb Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 21 Nov 2006 00:03:56 +0000 Subject: catch up with latest Bullet 2.x (not in use yet) --- .../BroadphaseCollision/btDispatcher.h | 4 +- extern/bullet2/src/BulletCollision/CMakeLists.txt | 2 + .../CollisionDispatch/SphereTriangleDetector.h | 3 +- .../CollisionDispatch/btCollisionWorld.cpp | 8 +- .../CollisionDispatch/btCollisionWorld.h | 4 +- .../CollisionDispatch/btConvexConvexAlgorithm.cpp | 34 +- .../CollisionShapes/btConeShape.cpp | 53 +- .../BulletCollision/CollisionShapes/btConeShape.h | 24 +- .../CollisionShapes/btTriangleBuffer.cpp | 1 + .../CollisionShapes/btTriangleBuffer.h | 3 +- .../NarrowPhaseCollision/btGjkEpa.cpp | 709 +++++++++++++++++++++ .../NarrowPhaseCollision/btGjkEpa.h | 50 ++ .../btGjkEpaPenetrationDepthSolver.cpp | 47 ++ .../btGjkEpaPenetrationDepthSolver.h | 39 ++ .../NarrowPhaseCollision/btGjkPairDetector.cpp | 38 +- .../ConstraintSolver/btContactConstraint.cpp | 10 +- .../btSequentialImpulseConstraintSolver.cpp | 129 +++- .../btSequentialImpulseConstraintSolver.h | 33 + .../Dynamics/btDiscreteDynamicsWorld.cpp | 21 +- .../Dynamics/btDiscreteDynamicsWorld.h | 7 + .../src/BulletDynamics/Dynamics/btDynamicsWorld.h | 6 + .../Dynamics/btSimpleDynamicsWorld.cpp | 7 +- extern/bullet2/src/LinearMath/btGeometryUtil.cpp | 3 +- extern/bullet2/src/LinearMath/btGeometryUtil.h | 3 +- 24 files changed, 1155 insertions(+), 83 deletions(-) create mode 100644 extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp create mode 100644 extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h create mode 100644 extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp create mode 100644 extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h (limited to 'extern/bullet2') diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index bbc5bd3f5d0..415c398a101 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -38,7 +38,9 @@ struct btDispatcherInfo DISPATCH_CONTINUOUS }; btDispatcherInfo() - :m_dispatchFunc(DISPATCH_DISCRETE), + :m_timeStep(0.f), + m_stepCount(0), + m_dispatchFunc(DISPATCH_DISCRETE), m_timeOfImpact(1.f), m_useContinuous(false), m_debugDraw(0), diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt index 3169f15199d..e565bf7edea 100644 --- a/extern/bullet2/src/BulletCollision/CMakeLists.txt +++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt @@ -47,6 +47,8 @@ ADD_LIBRARY(LibBulletCollision CollisionShapes/btTriangleMesh.cpp CollisionShapes/btTriangleMeshShape.cpp NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btGjkEpa.cpp + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp NarrowPhaseCollision/btConvexCast.cpp NarrowPhaseCollision/btGjkConvexCast.cpp NarrowPhaseCollision/btGjkPairDetector.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 22262340222..8fedba19922 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -45,4 +45,5 @@ private: }; -#endif //SPHERE_TRIANGLE_DETECTOR_H \ No newline at end of file +#endif //SPHERE_TRIANGLE_DETECTOR_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 7edd3e8abfe..775ed3e13d8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -121,13 +121,13 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho } -void btCollisionWorld::performDiscreteCollisionDetection() + + + +void btCollisionWorld::performDiscreteCollisionDetection(btDispatcherInfo& dispatchInfo) { BEGIN_PROFILE("performDiscreteCollisionDetection"); - btDispatcherInfo dispatchInfo; - dispatchInfo.m_timeStep = 0.f; - dispatchInfo.m_stepCount = 0; //update aabb (of all moved objects) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index e2b8eb11238..e212e520bda 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -236,8 +236,8 @@ public: void removeCollisionObject(btCollisionObject* collisionObject); - virtual void performDiscreteCollisionDetection(); - + virtual void performDiscreteCollisionDetection( btDispatcherInfo& dispatchInfo); + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 36b0d15eac5..e896508bdb9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -39,7 +39,9 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" -//#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" + #ifdef WIN32 #if _MSC_VER >= 1310 @@ -59,7 +61,7 @@ subject to the following restrictions: #endif //USE_HULL -bool gUseEpa = false; +bool gUseEpa = true; #ifdef WIN32 @@ -113,13 +115,10 @@ void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +static btGjkEpaPenetrationDepthSolver gEpaPenetrationDepthSolver; static btMinkowskiPenetrationDepthSolver gPenetrationDepthSolver; -//static EpaPenetrationDepthSolver gEpaPenetrationDepthSolver; -#ifdef USE_EPA -Solid3EpaPenetrationDepth gSolidEpaPenetrationSolver; -#endif //USE_EPA void btConvexConvexAlgorithm::checkPenetrationDepthSolver() { @@ -128,10 +127,7 @@ void btConvexConvexAlgorithm::checkPenetrationDepthSolver() m_useEpa = gUseEpa; if (m_useEpa) { - - // m_gjkPairDetector.setPenetrationDepthSolver(&gEpaPenetrationDepthSolver); - - + m_gjkPairDetector.setPenetrationDepthSolver(&gEpaPenetrationDepthSolver); } else { m_gjkPairDetector.setPenetrationDepthSolver(&gPenetrationDepthSolver); @@ -153,8 +149,21 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); m_ownManifold = true; } + resultOut->setPersistentManifold(m_manifoldPtr); - +#ifdef USE_BT_GJKEPA + btConvexShape* shape0(static_cast(body0->getCollisionShape())); + btConvexShape* shape1(static_cast(body1->getCollisionShape())); + const btScalar radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/); + btGjkEpaSolver::sResults results; + if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(), + shape1,body1->getWorldTransform(), + radialmargin,results)) + { + dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + } +#else checkPenetrationDepthSolver(); btConvexShape* min0 = static_cast(body0->getCollisionShape()); @@ -173,8 +182,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); - resultOut->setPersistentManifold(m_manifoldPtr); m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); +#endif } @@ -274,3 +283,4 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC return resultFraction; } + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 13875fc5fe6..4cf11f1a977 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -16,50 +16,71 @@ subject to the following restrictions: #include "btConeShape.h" #include "LinearMath/btPoint3.h" -#ifdef WIN32 -static int coneindices[3] = {1,2,0}; -#else -static int coneindices[3] = {2,1,0}; -#endif + btConeShape::btConeShape (btScalar radius,btScalar height): m_radius (radius), m_height(height) { + setConeUpIndex(1); btVector3 halfExtents; m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height)); } +///choose upAxis index +void btConeShape::setConeUpIndex(int upIndex) +{ + switch (upIndex) + { + case 0: + m_coneIndices[0] = 1; + m_coneIndices[1] = 0; + m_coneIndices[2] = 2; + break; + case 1: + m_coneIndices[0] = 0; + m_coneIndices[1] = 1; + m_coneIndices[2] = 2; + break; + case 2: + m_coneIndices[0] = 0; + m_coneIndices[1] = 2; + m_coneIndices[2] = 1; + break; + default: + assert(0); + }; +} btVector3 btConeShape::coneLocalSupport(const btVector3& v) const { float halfHeight = m_height * 0.5f; - if (v[coneindices[1]] > v.length() * m_sinAngle) + if (v[m_coneIndices[1]] > v.length() * m_sinAngle) { btVector3 tmp; - tmp[coneindices[0]] = 0.f; - tmp[coneindices[1]] = halfHeight; - tmp[coneindices[2]] = 0.f; + tmp[m_coneIndices[0]] = 0.f; + tmp[m_coneIndices[1]] = halfHeight; + tmp[m_coneIndices[2]] = 0.f; return tmp; } else { - btScalar s = btSqrt(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]); + btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); if (s > SIMD_EPSILON) { btScalar d = m_radius / s; btVector3 tmp; - tmp[coneindices[0]] = v[coneindices[0]] * d; - tmp[coneindices[1]] = -halfHeight; - tmp[coneindices[2]] = v[coneindices[2]] * d; + tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; return tmp; } else { btVector3 tmp; - tmp[coneindices[0]] = 0.f; - tmp[coneindices[1]] = -halfHeight; - tmp[coneindices[2]] = 0.f; + tmp[m_coneIndices[0]] = 0.f; + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = 0.f; return tmp; } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 0fd3ce177fe..52d925fe714 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -19,7 +19,7 @@ subject to the following restrictions: #include "btConvexShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -/// implements cone shape interface +///btConeShape implements a Cone shape, around the Y axis class btConeShape : public btConvexShape { @@ -27,7 +27,7 @@ class btConeShape : public btConvexShape float m_sinAngle; float m_radius; float m_height; - + int m_coneIndices[3]; btVector3 coneLocalSupport(const btVector3& v) const; @@ -76,8 +76,28 @@ public: { return "Cone"; } + + ///choose upAxis index + void setConeUpIndex(int upIndex); + + int getConeUpIndex() const + { + return m_coneIndices[1]; + } }; +///btConeShape implements a Cone shape, around the X axis +class btConeShapeX : public btConeShape +{ + public: + btConeShapeX(btScalar radius,btScalar height); +}; +///btConeShapeZ implements a Cone shape, around the Z axis +class btConeShapeZ : public btConeShape +{ + public: + btConeShapeZ(btScalar radius,btScalar height); +}; #endif //CONE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp index 078ba38fdf4..54864c32f3a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp @@ -39,3 +39,4 @@ void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int trian m_triangleBuffer.push_back(tri); } + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index 4f421a616c8..d04341aa809 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -57,4 +57,5 @@ public: }; -#endif //BT_TRIANGLE_BUFFER_H \ No newline at end of file +#endif //BT_TRIANGLE_BUFFER_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp new file mode 100644 index 00000000000..55123d5adb6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp @@ -0,0 +1,709 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson +Nov.2006 +*/ + + +#include "btGjkEpa.h" +#include //for memset + +namespace gjkepa_impl +{ + +// +// Port. typedefs +// + +typedef btScalar F; +typedef bool Z; +typedef int I; +typedef unsigned int U; +typedef unsigned char U1; +typedef unsigned short U2; + +typedef btVector3 Vector3; +typedef btMatrix3x3 Rotation; + +// +// Config +// + +#if 0 +#define BTLOCALSUPPORT localGetSupportingVertexWithoutMargin +#else +#define BTLOCALSUPPORT localGetSupportingVertex +#endif + +// +// Const +// + +static const U chkPrecision =1/U(sizeof(F)==4); + +static const F cstInf =F(1/sin(0.)); +static const F cstPi =F(acos(-1.)); +static const F cst2Pi =cstPi*2; + +static const U GJK_maxiterations =128; +static const U GJK_hashsize =1<<6; +static const U GJK_hashmask =GJK_hashsize-1; +static const F GJK_insimplex_eps =F(0.0001); +static const F GJK_sqinsimplex_eps =GJK_insimplex_eps*GJK_insimplex_eps; + +static const U EPA_maxiterations =256; +static const F EPA_inface_eps =F(0.01); +static const F EPA_accuracy =F(0.001); + +// +// Utils +// + +static inline F Abs(F v) { return(v<0?-v:v); } +static inline F Sign(F v) { return(F(v<0?-1:1)); } +template static inline void Swap(T& a,T& b) { T +t(a);a=b;b=t; } +template static inline T Min(const T& a,const T& b) { +return(a static inline T Max(const T& a,const T& b) { +return(a>b?a:b); } +static inline void ClearMemory(void* p,U sz) { memset(p,0,(size_t)sz); +} +#if 0 +template static inline void Raise(const T& object) { +throw(object); } +#else +template static inline void Raise(const T&) {} +#endif +static inline F Det(const Vector3& a,const Vector3& b,const Vector3& +c,const Vector3& d) + { + return( -(a.z()*b.y()*c.x()) + a.y()*b.z()*c.x() + a.z()*b.x()*c.y() - +a.x()*b.z()*c.y() - a.y()*b.x()*c.z() + a.x()*b.y()*c.z() + + a.z()*b.y()*d.x() - a.y()*b.z()*d.x() - a.z()*c.y()*d.x() + +b.z()*c.y()*d.x() + a.y()*c.z()*d.x() - b.y()*c.z()*d.x() - + a.z()*b.x()*d.y() + a.x()*b.z()*d.y() + a.z()*c.x()*d.y() - +b.z()*c.x()*d.y() - a.x()*c.z()*d.y() + b.x()*c.z()*d.y() + + a.y()*b.x()*d.z() - a.x()*b.y()*d.z() - a.y()*c.x()*d.z() + +b.y()*c.x()*d.z() + a.x()*c.y()*d.z() - b.x()*c.y()*d.z()); + } + +// +// StackAlloc +// +struct StackAlloc + { + struct Block + { + Block* previous; + U1* address; + }; + StackAlloc() { ctor(); } + StackAlloc(U size) { ctor();Create(size); } + ~StackAlloc() { Free(); } + void Create(U size) + { + Free(); + data = new U1[size]; + totalsize = size; + } + StackAlloc* CreateChild(U size) + { + StackAlloc* sa(Allocate()); + sa->ischild = true; + sa->data = Allocate(size); + sa->totalsize = size; + sa->usedsize = 0; + sa->current = 0; + return(sa); + } + void Free() + { + if(usedsize==0) + { + if(!ischild) delete[] data; + data = 0; + usedsize = 0; + } else Raise(L"StackAlloc is still in use"); + } + Block* BeginBlock() + { + Block* pb(Allocate()); + pb->previous = current; + pb->address = data+usedsize; + current = pb; + return(pb); + } + void EndBlock(Block* block) + { + if(block==current) + { + current = block->previous; + usedsize = (U)((block->address-data)-sizeof(Block)); + } else Raise(L"Unmatched blocks"); + } + U1* Allocate(U size) + { + const U nus(usedsize+size); + if(nus T* Allocate() { +return((T*)Allocate((U)sizeof(T))); } + template T* AllocateArray(U count) { +return((T*)Allocate((U)sizeof(T)*count)); } + private: + void ctor() + { + data = 0; + totalsize = 0; + usedsize = 0; + current = 0; + ischild = false; + } + U1* data; + U totalsize; + U usedsize; + Block* current; + Z ischild; + }; + +// +// GJK +// +struct GJK + { + struct Mkv + { + Vector3 w; /* Minkowski vertice */ + Vector3 r; /* Ray */ + }; + struct He + { + Vector3 v; + He* n; + }; + static const U hashsize=64; + StackAlloc* sa; + StackAlloc::Block* sablock; + He* table[hashsize]; + Rotation wrotations[2]; + Vector3 positions[2]; + const btConvexShape* shapes[2]; + Mkv simplex[5]; + Vector3 ray; + U order; + U iterations; + F margin; + Z failed; + // + GJK(StackAlloc* psa, + const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0, + const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1, + F pmargin=0) + { + wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0; + wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1; + sa =psa; + sablock =sa->BeginBlock(); + margin =pmargin; + failed =false; + } + // + ~GJK() + { + sa->EndBlock(sablock); + } + // vdh : very dumm hash + static inline U Hash(const Vector3& v) + { + const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473)); + return(((*((const U*)&h))*169639)&GJK_hashmask); + } + // + inline Vector3 LocalSupport(const Vector3& d,U i) const + { + return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]); + } + // + inline void Support(const Vector3& d,Mkv& v) const + { + v.r = d; + v.w = LocalSupport(d,0)-LocalSupport(-d,1)+d*margin; + } + #define SPX(_i_) simplex[_i_] + #define SPXW(_i_) simplex[_i_].w + // + inline Z FetchSupport() + { + const U h(Hash(ray)); + He* e(table[h]); + while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; } + e=sa->Allocate();e->v=ray;e->n=table[h];table[h]=e; + Support(ray,simplex[++order]); + return(ray.dot(SPXW(order))>0); + } + // + inline Z SolveSimplex2(const Vector3& ao,const Vector3& ab) + { + if(ab.dot(ao)>=0) + { + const Vector3 cabo(cross(ab,ao)); + if(cabo.length2()>GJK_sqinsimplex_eps) + { ray=cross(cabo,ab); } + else + { return(true); } + } + else + { order=0;SPX(0)=SPX(1);ray=ao; } + return(false); + } + // + inline Z SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& +ac) + { + return(SolveSimplex3a(ao,ab,ac,cross(ab,ac))); + } + // + inline Z SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& +ac,const Vector3& cabc) + { + if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps) + { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab)); } + else if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps) + { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); } + else + { + const F d(cabc.dot(ao)); + if(Abs(d)>GJK_insimplex_eps) + { + if(d>0) + { ray=cabc; } + else + { ray=-cabc;Swap(SPX(0),SPX(1)); } + return(false); + } else return(true); + } + } + // + inline Z SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& +ac,const Vector3& ad) + { + Vector3 crs; + if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps) + { +order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); +} + else if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps) + { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); } + else if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps) + { +order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); +} + else return(true); + } + // + inline Z SearchOrigin(const Vector3& initray=Vector3(1,0,0)) + { + iterations = 0; + order = 0; + failed = false; + Support(initray,simplex[0]);ray=-SPXW(0); + ClearMemory(table,sizeof(void*)*hashsize); + for(;iterations0?rl:1; + if(FetchSupport()) + { + Z found(false); + switch(order) + { + case 1: found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break; + case 2: found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break; + case 3: found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break; + } + if(found) return(true); + } else return(false); + } + failed=true; + return(false); + } + // + inline Z EncloseOrigin() + { + switch(order) + { + /* Point */ + case 0: break; + /* Line */ + case 1: + { + const Vector3 ab(SPXW(1)-SPXW(0)); + const Vector3 b[]={ cross(ab,Vector3(1,0,0)), + cross(ab,Vector3(0,1,0)), + cross(ab,Vector3(0,0,1))}; + const F m[]={b[0].length2(),b[1].length2(),b[2].length2()}; + const Rotation r(btQuaternion(ab.normalized(),cst2Pi/3)); + Vector3 w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]); + Support(w.normalized(),simplex[4]);w=r*w; + Support(w.normalized(),simplex[2]);w=r*w; + Support(w.normalized(),simplex[3]);w=r*w; + order=4; + return(true); + } + break; + /* Triangle */ + case 2: + { + const +Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized()); + Support( n,simplex[3]); + Support(-n,simplex[4]); + order=4; + return(true); + } + break; + /* Tetrahedron */ + case 3: return(true); + /* Hexahedron */ + case 4: return(true); + } + return(false); + } + #undef SPX + #undef SPXW + }; + +// +// EPA +// +struct EPA + { + // + struct Face + { + const GJK::Mkv* v[3]; + Face* f[3]; + U e[3]; + Vector3 n; + F d; + U mark; + Face* prev; + Face* next; + Face() {} + }; + // + GJK* gjk; + StackAlloc* sa; + Face* root; + U nfaces; + U iterations; + Vector3 features[2][3]; + Vector3 nearest[2]; + Vector3 normal; + F depth; + Z failed; + // + EPA(GJK* pgjk) + { + gjk = pgjk; + sa = pgjk->sa; + } + // + ~EPA() + { + } + // + inline Vector3 GetCoordinates(const Face* face) const + { + const Vector3 o(face->n*-face->d); + const F a[]={ cross(face->v[0]->w-o,face->v[1]->w-o).length(), + cross(face->v[1]->w-o,face->v[2]->w-o).length(), + cross(face->v[2]->w-o,face->v[0]->w-o).length()}; + const F sm(a[0]+a[1]+a[2]); + return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1)); + } + // + inline Face* FindBest() const + { + Face* bf(0); + if(root) + { + Face* cf(root); + F bd(cstInf); + do { + if(cf->dd;bf=cf; } + } while(0!=(cf=cf->next)); + } + return(bf); + } + // + inline Z Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* +c) const + { + const Vector3 nrm(cross(b->w-a->w,c->w-a->w)); + const F len(nrm.length()); + const Z valid( (cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&& + (cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&& + (cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps)); + f->v[0] = a; + f->v[1] = b; + f->v[2] = c; + f->mark = 0; + f->n = nrm/(len>0?len:cstInf); + f->d = Max(0,-f->n.dot(a->w)); + return(valid); + } + // + inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) + { + Face* pf(sa->Allocate()); + if(Set(pf,a,b,c)) + { + if(root) root->prev=pf; + pf->prev=0; + pf->next=root; + root =pf; + ++nfaces; + } + else + { + pf->prev=pf->next=0; + } + return(pf); + } + // + inline void Detach(Face* face) + { + if(face->prev||face->next) + { + --nfaces; + if(face==root) + { root=face->next;root->prev=0; } + else + { + if(face->next==0) + { face->prev->next=0; } + else + { face->prev->next=face->next;face->next->prev=face->prev; } + } + face->prev=face->next=0; + } + } + // + inline void Link(Face* f0,U e0,Face* f1,U e1) const + { + f0->f[e0]=f1;f1->e[e1]=e0; + f1->f[e1]=f0;f0->e[e0]=e1; + } + // + GJK::Mkv* Support(const Vector3& w) const + { + GJK::Mkv* v(sa->Allocate()); + gjk->Support(w,*v); + return(v); + } + // + U BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& +ff) + { + static const U mod3[]={0,1,2,0,1}; + U ne(0); + if(f.mark!=markid) + { + 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)); + Link(nf,0,&f,e); + if(cf) Link(cf,1,nf,2); else ff=nf; + cf=nf;ne=1; + } + else + { + const U e2(mod3[e+2]); + Detach(&f); + f.mark = markid; + ne += BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff); + ne += BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff); + } + } + return(ne); + } + // + inline F EvaluatePD(F accuracy=EPA_accuracy) + { + StackAlloc::Block* sablock(sa->BeginBlock()); + Face* bestface(0); + U markid(1); + depth = -cstInf; + normal = Vector3(0,0,0); + root = 0; + nfaces = 0; + iterations = 0; + failed = false; + /* Prepare hull */ + if(gjk->EncloseOrigin()) + { + const U* pfidx(0); + U nfidx(0); + const U* peidx(0); + U neidx(0); + GJK::Mkv* basemkv[5]; + Face* basefaces[6]; + switch(gjk->order) + { + /* Tetrahedron */ + case 3: { + static const U fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}}; + static const +U eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}}; + pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6; + } break; + /* Hexahedron */ + case 4: { + static const +U fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; + static const +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) { +basemkv[i]=sa->Allocate();*basemkv[i]=gjk->simplex[i]; } + for(U i=0;iEndBlock(sablock); + return(depth); + } + /* Expand hull */ + for(;iterationsn)); + const F d(bf->n.dot(w->w)+bf->d); + bestface = bf; + if(d<-accuracy) + { + Face* cf(0); + Face* ff(0); + U nf(0); + Detach(bf); + bf->mark=++markid; + for(U i=0;i<3;++i) { +nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); } + if(nf<=2) { break; } + Link(cf,1,ff,2); + } else break; + } else break; + } + /* Extract contact */ + if(bestface) + { + const Vector3 b(GetCoordinates(bestface)); + normal = bestface->n; + depth = Max(0,bestface->d); + for(U i=0;i<2;++i) + { + const F s(F(i?-1:1)); + for(U j=0;j<3;++j) + { + features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i); + } + } + nearest[0] = features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z(); + nearest[1] = features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z(); + } else failed=true; + sa->EndBlock(sablock); + return(depth); + } + }; +} + +// +// Api +// + +using namespace gjkepa_impl; + +/* Need some kind of stackalloc , create a static one till bullet provide +one. */ +static const U g_sasize((1024<<10)*2); +static StackAlloc g_sa(g_sasize); + +// +bool btGjkEpaSolver::Collide(btConvexShape *shape0,const btTransform &wtrs0, + btConvexShape *shape1,const btTransform &wtrs1, + btScalar radialmargin, + sResults& results) +{ +/* Initialize */ +results.witnesses[0] = +results.witnesses[1] = +results.normal = Vector3(0,0,0); +results.depth = 0; +results.status = sResults::Separated; +results.epa_iterations = 0; +results.gjk_iterations = 0; +/* Use GJK to locate origin */ +GJK gjk(&g_sa, + wtrs0.getBasis(),wtrs0.getOrigin(),shape0, + wtrs1.getBasis(),wtrs1.getOrigin(),shape1, + radialmargin+EPA_accuracy); +const Z collide(gjk.SearchOrigin()); +results.gjk_iterations = gjk.iterations+1; +if(collide) + { + /* Then EPA for penetration depth */ + EPA epa(&gjk); + const F pd(epa.EvaluatePD()); + results.epa_iterations = epa.iterations+1; + if(pd>0) + { + results.status = sResults::Penetrating; + results.normal = epa.normal; + results.depth = pd; + results.witnesses[0] = epa.nearest[0]; + results.witnesses[1] = epa.nearest[1]; + return(true); + } else { if(epa.failed) results.status=sResults::EPA_Failed; } + } else { if(gjk.failed) results.status=sResults::GJK_Failed; } +return(false); +} + + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h new file mode 100644 index 00000000000..d26ac9cdfe5 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h @@ -0,0 +1,50 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson +Nov.2006 +*/ + + +#ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ +#define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct btGjkEpaSolver +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar depth; + int epa_iterations; + int gjk_iterations; + }; +static bool Collide(btConvexShape* shape0,const btTransform& wtrs0, + btConvexShape* shape1,const btTransform& wtrs1, + btScalar radialmargin, + sResults& results); +}; + +#endif diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp new file mode 100644 index 00000000000..c2958b22b6a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "btGjkEpaPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" + +bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, + btConvexShape* pConvexA, btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, + class btIDebugDraw* debugDraw ) +{ + + + const btScalar radialmargin(0.f); + + btGjkEpaSolver::sResults results; + if(btGjkEpaSolver::Collide( pConvexA,transformA, + pConvexB,transformB, + radialmargin,results)) + { + // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + return true; + } + + return false; +} + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h new file mode 100644 index 00000000000..a7332a7a51a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -0,0 +1,39 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H +#define BT_GJP_EPA_PENETRATION_DEPTH_H + +#include "btConvexPenetrationDepthSolver.h" + +///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to +///calculate the penetration depth between two convex shapes. +class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver +{ + public : + + bool calcPenDepth( btSimplexSolverInterface& simplexSolver, + btConvexShape* pConvexA, btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, + class btIDebugDraw* debugDraw ); + + private : + +}; + +#endif // BT_GJP_EPA_PENETRATION_DEPTH_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 507fcaf21f7..646c07a5d39 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -25,6 +25,9 @@ subject to the following restrictions: //must be above the machine epsilon #define REL_ERROR2 1.0e-6f +//temp globals, to improve GJK/EPA/penetration calculations +int gNumDeepPenetrationChecks = 0; +int gNumGjkChecks = 0; #ifdef __SPU__ #include @@ -39,7 +42,7 @@ m_minkowskiA(objectA), m_minkowskiB(objectB), m_ignoreMargin(false), m_lastUsedMethod(-1), -m_catchDegeneracies(0) +m_catchDegeneracies(1) { } @@ -57,6 +60,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& float marginA = m_minkowskiA->getMargin(); float marginB = m_minkowskiB->getMargin(); + gNumGjkChecks++; + //for CCD we don't use margins if (m_ignoreMargin) { @@ -71,7 +76,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& bool isValid = false; bool checkSimplex = false; bool checkPenetration = true; - m_degenerateSimplex = false; + m_degenerateSimplex = 0; m_lastUsedMethod = -1; @@ -109,6 +114,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& //exit 0: the new point is already in the simplex, or we didn't come any closer if (m_simplexSolver->inSimplex(w)) { + m_degenerateSimplex = 1; checkSimplex = true; break; } @@ -116,14 +122,12 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& float f0 = squaredDistance - delta; float f1 = squaredDistance * REL_ERROR2; - if (f0 <= 0.f) - { - m_degenerateSimplex = 2; - } - - if (f0 >= 0.f && (f0 <= f1)) - //if (f0 <= f1) + if (f0 <= f1) { + if (f0 <= 0.f) + { + m_degenerateSimplex = 2; + } checkSimplex = true; break; } @@ -133,7 +137,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& //calculate the closest point to the origin (update vector v) if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) { - m_degenerateSimplex = 1; + m_degenerateSimplex = 3; checkSimplex = true; break; } @@ -188,7 +192,10 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& normalInB = pointOnA-pointOnB; float lenSqr = m_cachedSeparatingAxis.length2(); //valid normal - //if (lenSqr > (0.1f*margin)) //SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr < 0.0001) + { + m_degenerateSimplex = 5; + } if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) { float rlen = 1.f / btSqrt(lenSqr ); @@ -200,6 +207,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& pointOnB += m_cachedSeparatingAxis * (marginB / s); distance = ((1.f/rlen) - margin); isValid = true; + m_lastUsedMethod = 1; } else { @@ -207,8 +215,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } } + bool catchDegeneratePenetrationCase = + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); + //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex) )) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) { //penetration case @@ -217,6 +228,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { // Penetration depth case. btVector3 tmpPointOnA,tmpPointOnB; + + gNumDeepPenetrationChecks++; + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( *m_simplexSolver, m_minkowskiA,m_minkowskiB, diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 8d22689ec53..1b85a0eea42 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -104,7 +104,7 @@ float resolveSingleCollision( btScalar Kfps = 1.f / solverInfo.m_timeStep ; - float damping = solverInfo.m_damping ; + // float damping = solverInfo.m_damping ; float Kerp = solverInfo.m_erp; float Kcor = Kerp *Kfps; @@ -136,7 +136,7 @@ float resolveSingleCollision( { body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse); } -#else USE_INTERNAL_APPLY_IMPULSE +#else //USE_INTERNAL_APPLY_IMPULSE body1.applyImpulse(normal*(normalImpulse), rel_pos1); body2.applyImpulse(-normal*(normalImpulse), rel_pos2); #endif //USE_INTERNAL_APPLY_IMPULSE @@ -215,7 +215,7 @@ float resolveSingleFriction( body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1); body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2); } -#else USE_INTERNAL_APPLY_IMPULSE +#else //USE_INTERNAL_APPLY_IMPULSE body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1); body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2); #endif //USE_INTERNAL_APPLY_IMPULSE @@ -317,7 +317,7 @@ float resolveSingleCollisionCombined( btScalar Kfps = 1.f / solverInfo.m_timeStep ; - float damping = solverInfo.m_damping ; + //float damping = solverInfo.m_damping ; float Kerp = solverInfo.m_erp; float Kcor = Kerp *Kfps; @@ -350,7 +350,7 @@ float resolveSingleCollisionCombined( { body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse); } -#else USE_INTERNAL_APPLY_IMPULSE +#else //USE_INTERNAL_APPLY_IMPULSE body1.applyImpulse(normal*(normalImpulse), rel_pos1); body2.applyImpulse(-normal*(normalImpulse), rel_pos2); #endif //USE_INTERNAL_APPLY_IMPULSE diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 568bcc40939..cf33b1d674e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -63,8 +63,14 @@ bool MyContactDestroyedCallback(void* userPersistentData) return true; } +btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2() +{ + setSolverMode(SOLVER_USE_WARMSTARTING); +} + btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +:m_solverMode(SOLVER_RANDMIZE_ORDER) { gContactDestroyedCallback = &MyContactDestroyedCallback; @@ -92,57 +98,127 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man int totalPoints = 0; + { int j; for (j=0;jgetNumContacts();p++) + { + gOrder[totalPoints].m_manifoldIndex = j; + gOrder[totalPoints].m_pointIndex = p; + totalPoints++; + } } } + + //should traverse the contacts random order... + int iteration; + + { + for ( iteration = 0;iterationgetBody0(), + (btRigidBody*)manifold->getBody1() + ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + } + + for (j=0;jgetBody0(), + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + } + } + } + +#ifdef USE_PROFILE + btProfiler::endBlock("solve"); +#endif //USE_PROFILE + + return 0.f; +} + + +/// btSequentialImpulseConstraintSolver Sequentially applies impulses +float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + + btContactSolverInfo info = infoGlobal; + + int numiter = infoGlobal.m_numIterations; +#ifdef USE_PROFILE + btProfiler::beginBlock("solve"); +#endif //USE_PROFILE { int j; for (j=0;jgetNumContacts();p++) { - gOrder[totalPoints].m_manifoldIndex = j; - gOrder[totalPoints].m_pointIndex = p; - totalPoints++; + //interleaving here gives better results + solve( (btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1() + ,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer); } } } - //should traverse the contacts random order... int iteration; + for ( iteration = 0;iterationgetBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + btPersistentManifold* manifold = manifoldPtr[j]; + for (int p=0;pgetNumContacts();p++) + { + solve( (btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1() + ,manifold->getContactPoint(p),info,iteration,debugDrawer); + } } - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + btPersistentManifold* manifold = manifoldPtr[j]; + for (int p=0;pgetNumContacts();p++) + { + solveFriction((btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer); + } } } + #ifdef USE_PROFILE btProfiler::endBlock("solve"); @@ -264,7 +340,14 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol float relaxation = info.m_damping; - cpd->m_appliedImpulse =0.f;//*= relaxation; + if (m_solverMode & SOLVER_USE_WARMSTARTING) + { + cpd->m_appliedImpulse *= relaxation; + } else + { + cpd->m_appliedImpulse =0.f; + } + //for friction cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 8264f5071dd..bcc3806b923 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -30,6 +30,7 @@ class btIDebugDraw; class btSequentialImpulseConstraintSolver : public btConstraintSolver { +protected: float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); @@ -37,8 +38,18 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + //choose between several modes, different friction model etc. + int m_solverMode; + public: + enum eSolverMode + { + SOLVER_RANDMIZE_ORDER = 1, + SOLVER_FRICTION_SEPARATE = 2, + SOLVER_USE_WARMSTARTING = 4 + }; + btSequentialImpulseConstraintSolver(); ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody @@ -59,7 +70,29 @@ public: virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + void setSolverMode(int mode) + { + m_solverMode = mode; + } + + int getSolverMode() const + { + return m_solverMode; + } +}; + +/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering +class btSequentialImpulseConstraintSolver2 : public btSequentialImpulseConstraintSolver +{ +public: + + btSequentialImpulseConstraintSolver2(); + + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + + }; + #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 125e933ad90..bdde013aadb 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -267,8 +267,14 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) ///apply gravity, predict motion predictUnconstraintMotion(timeStep); + btDispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = getDebugDrawer(); + + ///perform collision detection - performDiscreteCollisionDetection(); + performDiscreteCollisionDetection(dispatchInfo); calculateSimulationIslands(); @@ -828,6 +834,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, } } + void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) @@ -838,3 +845,15 @@ void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) m_constraintSolver = solver; } +int btDiscreteDynamicsWorld::getNumConstraints() const +{ + return int(m_constraints.size()); +} +btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) +{ + return m_constraints[index]; +} +const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const +{ + return m_constraints[index]; +} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 12939753442..4aa83683cd5 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -137,6 +137,13 @@ public: void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); virtual void setConstraintSolver(btConstraintSolver* solver); + + virtual int getNumConstraints() const; + + virtual btTypedConstraint* getConstraint(int index) ; + + virtual const btTypedConstraint* getConstraint(int index) const; + }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 6023f29b4fb..a991fdca79d 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -68,6 +68,12 @@ class btDynamicsWorld : public btCollisionWorld virtual void removeRigidBody(btRigidBody* body) = 0; virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + + virtual int getNumConstraints() const { return 0; } + + virtual btTypedConstraint* getConstraint(int index) { return 0; } + + virtual const btTypedConstraint* getConstraint(int index) const { return 0; } }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index dabf291669d..fe6a73939a7 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -52,8 +52,13 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa ///apply gravity, predict motion predictUnconstraintMotion(timeStep); + btDispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = getDebugDrawer(); + ///perform collision detection - performDiscreteCollisionDetection(); + performDiscreteCollisionDetection(dispatchInfo ); ///solve contact constraints int numManifolds = m_dispatcher1->getNumManifolds(); diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp index 69d061d27d0..5176d317417 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp @@ -165,4 +165,5 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const std::vector& } } } -} \ No newline at end of file +} + diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h index 3170c215008..525291e6051 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.h +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.h @@ -36,4 +36,5 @@ class btGeometryUtil }; -#endif //BT_GEOMETRY_UTIL_H \ No newline at end of file +#endif //BT_GEOMETRY_UTIL_H + -- cgit v1.2.3