diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2006-11-21 03:03:56 +0300 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2006-11-21 03:03:56 +0300 |
commit | d58670ddd2e43c9ba6e56c766827165eb3ba08cb (patch) | |
tree | 96e85d2feeb35a9939297257ceccca9edd6f05b9 /extern/bullet2/src/BulletCollision | |
parent | 731e7e3915f35de124ab65b455316e5fd3e60585 (diff) |
catch up with latest Bullet 2.x (not in use yet)
Diffstat (limited to 'extern/bullet2/src/BulletCollision')
15 files changed, 968 insertions, 51 deletions
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<btConvexShape*>(body0->getCollisionShape())); + btConvexShape* shape1(static_cast<btConvexShape*>(body1->getCollisionShape())); + const btScalar radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/); + btGjkEpaSolver::sResults results; + if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(), + shape1,body1->getWorldTransform(), + radialmargin,results)) + { + dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + } +#else checkPenetrationDepthSolver(); btConvexShape* min0 = static_cast<btConvexShape*>(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 <string.h> //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 <typename T> static inline void Swap(T& a,T& b) { T +t(a);a=b;b=t; } +template <typename T> static inline T Min(const T& a,const T& b) { +return(a<b?a:b); } +template <typename T> 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 <typename T> static inline void Raise(const T& object) { +throw(object); } +#else +template <typename T> 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<StackAlloc>()); + 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<Block>()); + 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<totalsize) + { + usedsize=nus; + return(data+(usedsize-size)); + } + Raise(L"Not enough memory"); + return(0); + } + template <typename T> T* Allocate() { +return((T*)Allocate((U)sizeof(T))); } + template <typename T> 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<He>();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(;iterations<GJK_maxiterations;++iterations) + { + const F rl(ray.length()); + ray/=rl>0?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->d<bd) { bd=cf->d;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<F>(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<Face>()); + 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::Mkv>()); + 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<GJK::Mkv>();*basemkv[i]=gjk->simplex[i]; } + for(U i=0;i<nfidx;++i,pfidx+=3) { +basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); +} + for(U i=0;i<neidx;++i,peidx+=4) { +Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); } + } + if(0==nfaces) + { + sa->EndBlock(sablock); + return(depth); + } + /* Expand hull */ + for(;iterations<EPA_maxiterations;++iterations) + { + Face* bf(FindBest()); + if(bf) + { + 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); + 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<F>(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 <spu_printf.h> @@ -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, |