Changeset 7983 for code/branches/kicklib/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

Timestamp:

Feb 27, 2011, 7:43:24 AM (14 years ago)

Author:

rgrieder

Message:

Updated Bullet Physics Engine from v2.74 to v2.77

File:

• code/branches/kicklib/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp (modified) (20 diffs)

code/branches/kicklib/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -39 +39 @@
 
 
-
 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*  penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
 m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
 m_minkowskiA(objectA),
 m_minkowskiB(objectB),
+m_shapeTypeA(objectA->getShapeType()),
+m_shapeTypeB(objectB->getShapeType()),
+m_marginA(objectA->getMargin()),
+m_marginB(objectB->getMargin()),
 m_ignoreMargin(false),
 m_lastUsedMethod(-1),
@@ -51 +54 @@
 {
 }
-
-void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*        penetrationDepthSolver)
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_shapeTypeA(shapeTypeA),
+m_shapeTypeB(shapeTypeB),
+m_marginA(marginA),
+m_marginB(marginB),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+
+void    btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+        (void)swapResults;
+
+        getClosestPointsNonVirtual(input,output,debugDraw);
+}
+
+#ifdef __SPU__
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#else
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#endif
 {
         m_cachedSeparatingDistance = 0.f;
@@ -65 +94 @@
         localTransB.getOrigin() -= positionOffset;
 
-#ifdef __SPU__
-        btScalar marginA = m_minkowskiA->getMarginNonVirtual();
-        btScalar marginB = m_minkowskiB->getMarginNonVirtual();
-#else
-        btScalar marginA = m_minkowskiA->getMargin();
-        btScalar marginB = m_minkowskiB->getMargin();
-#ifdef TEST_NON_VIRTUAL
-        btScalar marginAv = m_minkowskiA->getMarginNonVirtual();
-        btScalar marginBv = m_minkowskiB->getMarginNonVirtual();
-        btAssert(marginA == marginAv);
-        btAssert(marginB == marginBv);
-#endif //TEST_NON_VIRTUAL
-#endif
-        
-
+        bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
+
+        btScalar marginA = m_marginA;
+        btScalar marginB = m_marginB;
 
         gNumGjkChecks++;
@@ -108 +126 @@
 
         {
-                btScalar squaredDistance = SIMD_INFINITY;
+                btScalar squaredDistance = BT_LARGE_FLOAT;
                 btScalar delta = btScalar(0.);
                 
@@ -124 +142 @@
                         btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
 
+#if 1
+
+                        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+                        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+//                      btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
+//                      btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
+
+#else
 #ifdef __SPU__
                         btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
@@ -137 +164 @@
 #endif //
 #endif //__SPU__
+#endif
+
 
                         btVector3  pWorld = localTransA(pInA);  
@@ -145 +174 @@
 #endif
 
+                        if (check2d)
+                        {
+                                pWorld[2] = 0.f;
+                                qWorld[2] = 0.f;
+                        }
+
                         btVector3 w     = pWorld - qWorld;
                         delta = m_cachedSeparatingAxis.dot(w);
@@ -151 +186 @@
                         if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
                         {
+                                m_degenerateSimplex = 10;
                                 checkSimplex=true;
                                 //checkPenetration = false;
@@ -172 +208 @@
                                 {
                                         m_degenerateSimplex = 2;
+                                } else
+                                {
+                                        m_degenerateSimplex = 11;
                                 }
                                 checkSimplex = true;
@@ -185 +224 @@
                 spu_printf("addVertex 2\n");
 #endif
+                        btVector3 newCachedSeparatingAxis;
+
                         //calculate the closest point to the origin (update vector v)
-                        if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+                        if (!m_simplexSolver->closest(newCachedSeparatingAxis))
                         {
                                 m_degenerateSimplex = 3;
@@ -193 +234 @@
                         }
 
-                        if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
+                        if(newCachedSeparatingAxis.length2()<REL_ERROR2)
             {
+                                m_cachedSeparatingAxis = newCachedSeparatingAxis;
                 m_degenerateSimplex = 6;
                 checkSimplex = true;
@@ -201 +243 @@
 
                         btScalar previousSquaredDistance = squaredDistance;
-                        squaredDistance = m_cachedSeparatingAxis.length2();
+                        squaredDistance = newCachedSeparatingAxis.length2();
+#if 0
+///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
+                        if (squaredDistance>previousSquaredDistance)
+                        {
+                                m_degenerateSimplex = 7;
+                                squaredDistance = previousSquaredDistance;
+                checkSimplex = false;
+                break;
+                        }
+#endif //
                         
+                        m_cachedSeparatingAxis = newCachedSeparatingAxis;
+
                         //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 
@@ -210 +264 @@
                                 m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
                                 checkSimplex = true;
+                                m_degenerateSimplex = 12;
+                                
                                 break;
                         }
@@ -240 +296 @@
                                 //do we need this backup_closest here ?
                                 m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+                                m_degenerateSimplex = 13;
                                 break;
                         }
@@ -248 +305 @@
                         m_simplexSolver->compute_points(pointOnA, pointOnB);
                         normalInB = pointOnA-pointOnB;
-                        btScalar lenSqr = m_cachedSeparatingAxis.length2();
+                        btScalar lenSqr =m_cachedSeparatingAxis.length2();
+                        
                         //valid normal
                         if (lenSqr < 0.0001)
@@ -280 +338 @@
                 {
                         //penetration case
-                
+
                         //if there is no way to handle penetrations, bail out
                         if (m_penetrationDepthSolver)
@@ -288 +346 @@
                                 
                                 gNumDeepPenetrationChecks++;
+                                m_cachedSeparatingAxis.setZero();
 
                                 bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
@@ -297 +356 @@
                                         );
 
+
                                 if (isValid2)
                                 {
                                         btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
                                         btScalar lenSqr = tmpNormalInB.length2();
+                                        if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+                                        {
+                                                tmpNormalInB = m_cachedSeparatingAxis;
+                                                lenSqr = m_cachedSeparatingAxis.length2();
+                                        }
+
                                         if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
                                         {
@@ -316 +382 @@
                                                 } else
                                                 {
-                                                        
+                                                        m_lastUsedMethod = 8;
                                                 }
                                         } else
                                         {
-                                                //isValid = false;
-                                                m_lastUsedMethod = 4;
+                                                m_lastUsedMethod = 9;
                                         }
                                 } else
+
                                 {
-                                        m_lastUsedMethod = 5;
+                                        ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+                                        ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+                                        ///reports a valid positive distance. Use the results of the second GJK instead of failing.
+                                        ///thanks to Jacob.Langford for the reproduction case
+                                        ///http://code.google.com/p/bullet/issues/detail?id=250
+
+                                
+                                        if (m_cachedSeparatingAxis.length2() > btScalar(0.))
+                                        {
+                                                btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
+                                                //only replace valid distances when the distance is less
+                                                if (!isValid || (distance2 < distance))
+                                                {
+                                                        distance = distance2;
+                                                        pointOnA = tmpPointOnA;
+                                                        pointOnB = tmpPointOnB;
+                                                        pointOnA -= m_cachedSeparatingAxis * marginA ;
+                                                        pointOnB += m_cachedSeparatingAxis * marginB ;
+                                                        normalInB = m_cachedSeparatingAxis;
+                                                        normalInB.normalize();
+                                                        isValid = true;
+                                                        m_lastUsedMethod = 6;
+                                                } else
+                                                {
+                                                        m_lastUsedMethod = 5;
+                                                }
+                                        }
                                 }
                                 
                         }
+
                 }
         }
 
-        if (isValid)
+        
+
+        if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
         {
-#ifdef __SPU__
-                //spu_printf("distance\n");
-#endif //__CELLOS_LV2__
-
-
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-                spu_printf("output 1\n");
-#endif
+#if 0
+///some debugging
+//              if (check2d)
+                {
+                        printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
+                        printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
+                }
+#endif 
+
                 m_cachedSeparatingAxis = normalInB;
                 m_cachedSeparatingDistance = distance;
@@ -350 +446 @@
                         distance);
 
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-                spu_printf("output 2\n");
-#endif
-                //printf("gjk add:%f",distance);
         }