From 86c27a7c9d03eb3f97199367bc1861be80423456 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Sat, 11 Nov 2006 23:59:51 +0000 Subject: [PATCH] Work on fixing some GJK issues reported by Pierre Terdiman (thanks Pierre for the testbed!) Improved this penetration test with more verbose output retrieve worldtransform from motionstate when rigidbody gets motionstate assigned --- Demos/BasicDemo/BasicDemo.cpp | 4 +- Demos/BspDemo/BspDemo.cpp | 1 + Demos/ConstraintDemo/ConstraintDemo.cpp | 2 +- .../EPAPenDepthDemo/PenetrationTestBullet.cpp | 132 ++++++++++++++- Demos/OpenGL/DemoApplication.cpp | 153 ++++++++++++------ Demos/OpenGL/DemoApplication.h | 3 + .../btGjkPairDetector.cpp | 71 ++++++-- .../NarrowPhaseCollision/btGjkPairDetector.h | 8 + .../btMinkowskiPenetrationDepthSolver.cpp | 56 ++++--- src/BulletDynamics/Dynamics/btRigidBody.h | 2 + 10 files changed, 331 insertions(+), 101 deletions(-) diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp index 8f4801759..9620a4cd0 100644 --- a/Demos/BasicDemo/BasicDemo.cpp +++ b/Demos/BasicDemo/BasicDemo.cpp @@ -129,7 +129,9 @@ void BasicDemo::initPhysics() m_solver = new btSequentialImpulseConstraintSolver; - m_dynamicsWorld = new btSimpleDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver); + //m_dynamicsWorld = new btSimpleDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver); + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver); + m_dynamicsWorld->setGravity(btVector3(0,-10,0)); m_dynamicsWorld->setDebugDrawer(&debugDrawer); diff --git a/Demos/BspDemo/BspDemo.cpp b/Demos/BspDemo/BspDemo.cpp index feb592c80..0989a3ada 100644 --- a/Demos/BspDemo/BspDemo.cpp +++ b/Demos/BspDemo/BspDemo.cpp @@ -135,6 +135,7 @@ void BspDemo::initPhysics(char* bspfilename) ///Setup a Physics Simulation Environment m_dynamicsWorld = new btDiscreteDynamicsWorld(); m_dynamicsWorld->setGravity(-m_cameraUp * 10); + m_dynamicsWorld->setDebugDrawer(&debugDrawer); #ifdef QUAKE_BSP_IMPORTING diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index 2ff91c6fd..cced57a7f 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -41,7 +41,7 @@ int main(int argc,char** argv) constraintDemo->initPhysics(); - constraintDemo->setCameraDistance(46.f); + constraintDemo->setCameraDistance(26.f); return glutmain(argc, argv,640,480,"Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/",constraintDemo); } diff --git a/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp b/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp index 5c9d8e7c9..2b2b3635f 100644 --- a/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp +++ b/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp @@ -20,6 +20,10 @@ #include #include +#define VERBOSE_TEXT_ONSCREEN 1 +#ifdef VERBOSE_TEXT_ONSCREEN +#include "BMF_Api.h" +#endif #include "btBulletCollisionCommon.h" @@ -35,6 +39,10 @@ static bool gRefMode = false; static int gMethod = 0; +static int gLastUsedMethod = -1; +static int gNumGjkIterations = -1; +static int gLastDegenerateSimplex = -1; + static const float gDisp = 0.01f; static const float gCamSpeed = 0.1f; static btVector3 Eye(3.0616338f, 1.1985892f, 2.5769043f); @@ -42,6 +50,8 @@ static btVector3 Dir(-0.66853905,-0.14004262,-0.73037237); static btVector3 N; static int mx = 0; static int my = 0; +static int glutScreenHeight = 512; +static int glutScreenWidth = 512; static void DrawLine(const btVector3& p0, const btVector3& p1, const btVector3& color, float line_width) { @@ -144,6 +154,36 @@ bool MyConvex::LoadFromFile(const char* filename) return true; } + + +//See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho +static void setOrthographicProjection() +{ + + // switch to projection mode + glMatrixMode(GL_PROJECTION); + // save previous matrix which contains the + //settings for the perspective projection + glPushMatrix(); + // reset matrix + glLoadIdentity(); + // set a 2D orthographic projection + gluOrtho2D(0, glutScreenWidth, 0, glutScreenHeight); + // invert the y axis, down is positive + glScalef(1, -1, 1); + // mover the origin from the bottom left corner + // to the upper left corner + glTranslatef(0, -glutScreenHeight, 0); + glMatrixMode(GL_MODELVIEW); +} + +static void resetPerspectiveProjection() +{ + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); +} + void MyConvex::Render(bool only_wireframe, const btVector3& wire_color) const { const float Scale = 1.0f; @@ -233,17 +273,19 @@ static float gDepth; static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1) { - //static btSimplexSolverInterface simplexSolver; - static Solid3JohnsonSimplexSolver simplexSolver; + static btSimplexSolverInterface simplexSolver; + //static Solid3JohnsonSimplexSolver simplexSolver; simplexSolver.reset(); btConvexHullShape convexA((float*)hull0.mVerts, hull0.mNbVerts, sizeof(btVector3)); btConvexHullShape convexB((float*)hull1.mVerts, hull1.mNbVerts, sizeof(btVector3)); - static Solid3EpaPenetrationDepth Solver0; - static EpaPenetrationDepthSolver Solver1; - static btMinkowskiPenetrationDepthSolver Solver2; + static btMinkowskiPenetrationDepthSolver Solver0; + static Solid3EpaPenetrationDepth Solver1; + static EpaPenetrationDepthSolver Solver2; + + btConvexPenetrationDepthSolver* Solver; if(gMethod==0) @@ -254,7 +296,7 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1) Solver = &Solver2; btGjkPairDetector GJK(&convexA, &convexB, &simplexSolver, Solver); - //GJK.setIgnoreMargin(true); + GJK.m_catchDegeneracies = 1; convexA.setMargin(0.01f); convexB.setMargin(0.01f); @@ -264,6 +306,9 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1) MyResult output; GJK.getClosestPoints(input, output, 0); + gLastUsedMethod = GJK.m_lastUsedMethod; + gNumGjkIterations = GJK.m_curIter; + gLastDegenerateSimplex= GJK.m_degenerateSimplex; return true; } @@ -539,6 +584,14 @@ static void RenderCallback() glEnable(GL_LIGHTING); + //clear previous frames result + gNormal.setValue(10,0,0); + gPoint.setValue(0,0,0); + gDepth = 999.999; + gLastUsedMethod = -1; + gNumGjkIterations = -1; + + TestEPA(gConvex0, gConvex1); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); @@ -552,6 +605,71 @@ static void RenderCallback() // DrawLine(gPoint, gPoint + gNormal*20.0f, btVector3(1,0,0), 2.0f); // printf("%f: %f %f %f\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z()); +#ifdef VERBOSE_TEXT_ONSCREEN + glColor3f(255.f, 255.f, 255.f); + + setOrthographicProjection(); + float xOffset = 10.f; + float yStart = 20.f; + float yIncr = 20.f; + char buf[124]; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"gDepth=%f: gNormal=(%f %f %f)\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z()); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"num GJK iterations =%d\n", gNumGjkIterations); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + + + + + if (gLastUsedMethod >= 3) + { + switch ( gMethod) + { + case 0: + sprintf(buf,"Minkowski sampling Penetration depth solver\n" ); + break; + case 1: + sprintf(buf,"Solid35 EPA Penetration depth solver\n" ); + break; + case 2: + sprintf(buf,"EPA Penetration depth solver (WorkInProgress, zlib free\n" ); + break; + default: + sprintf(buf,"Unknown Penetration Depth\n" ); + } + glRasterPos3f(xOffset,yStart,0); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + } else + { + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"Hybrid GJK method %d\n", gLastUsedMethod); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + } + + if (gLastDegenerateSimplex) + { + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"DegenerateSimplex %d\n", gLastDegenerateSimplex); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + } + + + + + resetPerspectiveProjection(); +#endif //VERBOSE_TEXT_ONSCREEN + btVector3 color(0,0,0); gConvex0.Render(false, color); gConvex1.Render(false, color); @@ -593,7 +711,7 @@ int main(int argc, char** argv) { // Initialize Glut glutInit(&argc, argv); - glutInitWindowSize(512, 512); + glutInitWindowSize(glutScreenWidth, glutScreenHeight); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); int mainHandle = glutCreateWindow("TestBullet"); glutSetWindow(mainHandle); diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 3055afe96..17cceb192 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -666,7 +666,33 @@ btRigidBody* DemoApplication::localCreateRigidBody(float mass, const btTransform return body; } +//See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho +void DemoApplication::setOrthographicProjection() +{ + // switch to projection mode + glMatrixMode(GL_PROJECTION); + // save previous matrix which contains the + //settings for the perspective projection + glPushMatrix(); + // reset matrix + glLoadIdentity(); + // set a 2D orthographic projection + gluOrtho2D(0, m_glutScreenWidth, 0, m_glutScreenHeight); + // invert the y axis, down is positive + glScalef(1, -1, 1); + // mover the origin from the bottom left corner + // to the upper left corner + glTranslatef(0, -m_glutScreenHeight, 0); + glMatrixMode(GL_MODELVIEW); +} + +void DemoApplication::resetPerspectiveProjection() +{ + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); +} void DemoApplication::renderme() @@ -726,11 +752,15 @@ void DemoApplication::renderme() float xOffset = 10.f; float yStart = 20.f; - float yIncr = -2.f; + float yIncr = 20.f; char buf[124]; glColor3f(0, 0, 0); + if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0) + { + setOrthographicProjection(); + #ifdef USE_QUICKPROF @@ -751,70 +781,83 @@ void DemoApplication::renderme() } #endif //USE_QUICKPROF - - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"mouse to interact"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; + + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"mouse to interact"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - /* glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"space to reset"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; - */ - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"cursor keys and z,x to navigate"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"space to reset"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"cursor keys and z,x to navigate"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"i to toggle simulation, s single step"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"i to toggle simulation, s single step"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"q to quit"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"q to quit"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,". to shoot box"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,". to shoot box"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - // not yet hooked up again after refactoring... + // not yet hooked up again after refactoring... -/* glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"d to toggle deactivation"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; -*/ + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"d to toggle deactivation"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - /* - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"a to draw temporal AABBs"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; - */ - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"h to toggle help text"); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; + /* + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"a to draw temporal AABBs"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + */ - //bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP); + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"h to toggle help text"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD); + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"p to toggle profiling (+results to file)"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; - glRasterPos3f(xOffset,yStart,0); - sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - yStart += yIncr; + //bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP); + + bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD); + +/* glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + */ + + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + resetPerspectiveProjection(); + } } @@ -838,6 +881,10 @@ void DemoApplication::clientResetScene() myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans; colObj->setWorldTransform( myMotionState->m_graphicsWorldTrans ); colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans ); + colObj->activate(); + //removed cached contact points + m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(colObj->getBroadphaseHandle()); + btRigidBody* body = btRigidBody::upcast(colObj); if (body && !body->isStaticObject()) { diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h index d00a32bf9..4af74af0d 100644 --- a/Demos/OpenGL/DemoApplication.h +++ b/Demos/OpenGL/DemoApplication.h @@ -93,6 +93,9 @@ public: return m_dynamicsWorld; } + void setOrthographicProjection(); + void resetPerspectiveProjection(); + int getDebugMode() { return m_debugMode ; diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index e61c15d93..507fcaf21 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -37,7 +37,9 @@ m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), -m_ignoreMargin(false) +m_ignoreMargin(false), +m_lastUsedMethod(-1), +m_catchDegeneracies(0) { } @@ -62,12 +64,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& marginB = 0.f; } - int curIter = 0; + m_curIter = 0; int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? + m_cachedSeparatingAxis.setValue(0,1,0); bool isValid = false; bool checkSimplex = false; bool checkPenetration = true; + m_degenerateSimplex = false; + + m_lastUsedMethod = -1; { btScalar squaredDistance = SIMD_INFINITY; @@ -107,7 +113,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& break; } // are we getting any closer ? - if (squaredDistance - delta <= squaredDistance * REL_ERROR2) + 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) { checkSimplex = true; break; @@ -118,6 +133,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; checkSimplex = true; break; } @@ -136,11 +152,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (curIter++ > gGjkMaxIter) + if (m_curIter++ > gGjkMaxIter) { #if defined(DEBUG) || defined (_DEBUG) - printf("btGjkPairDetector maxIter exceeded:%i\n",curIter); + printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", m_cachedSeparatingAxis.getX(), m_cachedSeparatingAxis.getY(), @@ -172,7 +188,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& normalInB = pointOnA-pointOnB; float lenSqr = m_cachedSeparatingAxis.length2(); //valid normal - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + //if (lenSqr > (0.1f*margin)) //SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) { float rlen = 1.f / btSqrt(lenSqr ); normalInB *= rlen; //normalize @@ -183,10 +200,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& pointOnB += m_cachedSeparatingAxis * (marginB / s); distance = ((1.f/rlen) - margin); isValid = true; + m_lastUsedMethod = 1; + } else + { + m_lastUsedMethod = 2; } } - if (checkPenetration && !isValid) + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex) )) { //penetration case @@ -194,27 +216,46 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (m_penetrationDepthSolver) { // Penetration depth case. - isValid = m_penetrationDepthSolver->calcPenDepth( + btVector3 tmpPointOnA,tmpPointOnB; + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( *m_simplexSolver, m_minkowskiA,m_minkowskiB, localTransA,localTransB, - m_cachedSeparatingAxis, pointOnA, pointOnB, + m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, debugDraw ); - if (isValid) + if (isValid2) { - normalInB = pointOnB-pointOnA; - float lenSqr = normalInB.length2(); + btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; + float lenSqr = tmpNormalInB.length2(); if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { - normalInB /= btSqrt(lenSqr); - distance = -(pointOnA-pointOnB).length(); + tmpNormalInB /= btSqrt(lenSqr); + float distance2 = -(tmpPointOnA-tmpPointOnB).length(); + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + m_lastUsedMethod = 3; + } else + { + + } } else { - isValid = false; + //isValid = false; + m_lastUsedMethod = 4; } + } else + { + m_lastUsedMethod = 5; } + } } } diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index c4842cd30..09c1669bd 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -43,6 +43,12 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface public: + //some debugging to fix degeneracy problems + int m_lastUsedMethod; + int m_curIter; + int m_degenerateSimplex; + int m_catchDegeneracies; + btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~btGjkPairDetector() {}; @@ -68,11 +74,13 @@ public: m_penetrationDepthSolver = penetrationDepthSolver; } + ///don't use setIgnoreMargin, it's for Bullet's internal use void setIgnoreMargin(bool ignoreMargin) { m_ignoreMargin = ignoreMargin; } + }; #endif //GJK_PAIR_DETECTOR_H diff --git a/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 6b2112ef3..74cf2b7c0 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -20,29 +20,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -struct MyResult : public btDiscreteCollisionDetectorInterface::Result -{ - MyResult():m_hasResult(false) - { - } - - btVector3 m_normalOnBInWorld; - btVector3 m_pointInWorld; - float m_depth; - bool m_hasResult; - - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) - { - } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) - { - m_normalOnBInWorld = normalOnBInWorld; - m_pointInWorld = pointInWorld; - m_depth = depth; - m_hasResult = true; - } -}; #define NUM_UNITSPHERE_POINTS 42 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = @@ -100,6 +78,31 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s ) { + + struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result + { + + btIntermediateResult():m_hasResult(false) + { + } + + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + float m_depth; + bool m_hasResult; + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + } + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + { + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + m_hasResult = true; + } + }; + //just take fixed number of orientation, and sample the penetration depth in that direction float minProj = 1e30f; btVector3 minNorm; @@ -247,10 +250,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minA += minNorm*convexA->getMargin(); minB -= minNorm*convexB->getMargin(); + //no penetration + if (minProj < 0.f) + return false; + minProj += (convexA->getMargin() + convexB->getMargin()); - + + //#define DEBUG_DRAW 1 #ifdef DEBUG_DRAW @@ -286,7 +294,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformB = transB; input.m_maximumDistanceSquared = 1e30f;//minProj; - MyResult res; + btIntermediateResult res; gjkdet.getClosestPoints(input,res,debugDraw); float correctedMinNorm = minProj - res.m_depth; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 62a29fa59..c5fecdb87 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -313,6 +313,8 @@ public: void setMotionState(btMotionState* motionState) { m_optionalMotionState = motionState; + if (m_optionalMotionState) + motionState->getWorldTransform(m_worldTransform); } //for experimental overriding of friction/contact solver func