catch up with latest Bullet 2.x (not in use yet)
authorErwin Coumans <blender@erwincoumans.com>
Tue, 21 Nov 2006 00:03:56 +0000 (00:03 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Tue, 21 Nov 2006 00:03:56 +0000 (00:03 +0000)
24 files changed:
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
extern/bullet2/src/BulletCollision/CMakeLists.txt
extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
extern/bullet2/src/LinearMath/btGeometryUtil.cpp
extern/bullet2/src/LinearMath/btGeometryUtil.h

index bbc5bd3..415c398 100644 (file)
@@ -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),
index 3169f15..e565bf7 100644 (file)
@@ -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
index 7edd3e8..775ed3e 100644 (file)
@@ -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)
 
index e2b8eb1..e212e52 100644 (file)
@@ -236,8 +236,8 @@ public:
 
        void    removeCollisionObject(btCollisionObject* collisionObject);
 
-       virtual void    performDiscreteCollisionDetection();
-
+       virtual void    performDiscreteCollisionDetection(      btDispatcherInfo&       dispatchInfo);
+       
 };
 
 
index 36b0d15..e896508 100644 (file)
@@ -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;
 
 }
+
index 13875fc..4cf11f1 100644 (file)
@@ -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;
        }
   }
index 0fd3ce1..52d925f 100644 (file)
@@ -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
 
index 078ba38..54864c3 100644 (file)
@@ -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/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
new file mode 100644 (file)
index 0000000..55123d5
--- /dev/null
@@ -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 (file)
index 0000000..d26ac9c
--- /dev/null
@@ -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 (file)
index 0000000..c2958b2
--- /dev/null
@@ -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 (file)
index 0000000..a7332a7
--- /dev/null
@@ -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
+
index 507fcaf..646c07a 100644 (file)
@@ -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,
index 8d22689..1b85a0e 100644 (file)
@@ -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
index 568bcc4..cf33b1d 100644 (file)
@@ -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;j<numManifolds;j++)
                {
-                       prepareConstraints(manifoldPtr[j],info,debugDrawer);
+                       btPersistentManifold* manifold = manifoldPtr[j];
+                       prepareConstraints(manifold,info,debugDrawer);
+                       for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
+                       {
+                               gOrder[totalPoints].m_manifoldIndex = j;
+                               gOrder[totalPoints].m_pointIndex = p;
+                               totalPoints++;
+                       }
                }
        }
+       
+       //should traverse the contacts random order...
+       int iteration;
+
+       {
+               for ( iteration = 0;iteration<numiter-1;iteration++)
+               {
+                       int j;
+                       if (m_solverMode & SOLVER_RANDMIZE_ORDER)
+                       {
+                               if ((iteration & 7) == 0) {
+                                       for (j=0; j<totalPoints; ++j) {
+                                               btOrderIndex tmp = gOrder[j];
+                                               int swapi = btRandInt2(j+1);
+                                               gOrder[j] = gOrder[swapi];
+                                               gOrder[swapi] = tmp;
+                                       }
+                               }
+                       }
+
+                       for (j=0;j<totalPoints;j++)
+                       {
+                               btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+                               solve( (btRigidBody*)manifold->getBody0(),
+                                                                       (btRigidBody*)manifold->getBody1()
+                               ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+                       }
+               
+                       for (j=0;j<totalPoints;j++)
+                       {
+                               btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+                               solveFriction((btRigidBody*)manifold->getBody0(),
+                                       (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;j<numManifolds;j++)
                {
+                       btPersistentManifold* manifold = manifoldPtr[j];
+                       prepareConstraints(manifold,info,debugDrawer);
                        for (int p=0;p<manifoldPtr[j]->getNumContacts();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;iteration<numiter-1;iteration++)
        {
                int j;
-               if ((iteration & 7) == 0) {
-                       for (j=0; j<totalPoints; ++j) {
-                               btOrderIndex tmp = gOrder[j];
-                               int swapi = btRandInt2(j+1);
-                               gOrder[j] = gOrder[swapi];
-                               gOrder[swapi] = tmp;
-                       }
-               }
 
-               for (j=0;j<totalPoints;j++)
+               for (j=0;j<numManifolds;j++)
                {
-                       btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-                       solve( (btRigidBody*)manifold->getBody0(),
-                                                               (btRigidBody*)manifold->getBody1()
-                       ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+                       btPersistentManifold* manifold = manifoldPtr[j];
+                       for (int p=0;p<manifold->getNumContacts();p++)
+                       {
+                               solve( (btRigidBody*)manifold->getBody0(),
+                                                                       (btRigidBody*)manifold->getBody1()
+                               ,manifold->getContactPoint(p),info,iteration,debugDrawer);
+                       }
                }
        
-               for (j=0;j<totalPoints;j++)
+       }
+
+       for ( iteration = 0;iteration<numiter-1;iteration++)
+       {
+               int j;
+               for (j=0;j<numManifolds;j++)
                {
-                       btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-                       solveFriction((btRigidBody*)manifold->getBody0(),
-                               (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+                       btPersistentManifold* manifold = manifoldPtr[j];
+                       for (int p=0;p<manifold->getNumContacts();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;
                                
index 8264f50..bcc3806 100644 (file)
@@ -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
 
index 125e933..bdde013 100644 (file)
@@ -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];
+}
index 1293975..4aa8368 100644 (file)
@@ -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;
+
 
 };
 
index 6023f29..a991fdc 100644 (file)
@@ -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;       }
 
 };
 
index dabf291..fe6a739 100644 (file)
@@ -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();
index 69d061d..5176d31 100644 (file)
@@ -165,4 +165,5 @@ void        btGeometryUtil::getVerticesFromPlaneEquations(const std::vector<btVector3>&
                        }
                }
        }
-}
\ No newline at end of file
+}
+
index 3170c21..525291e 100644 (file)
@@ -36,4 +36,5 @@ class btGeometryUtil
 };
 
 
-#endif //BT_GEOMETRY_UTIL_H
\ No newline at end of file
+#endif //BT_GEOMETRY_UTIL_H
+