Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletCollision / NarrowPhaseCollision / btGjkPairDetector.cpp
index 8877579496b06698219918fed07e6a46cc4ed697..759443a9613fd99a2a91de47c7828dce0bb12dff 100644 (file)
@@ -26,7 +26,6 @@ subject to the following restrictions:
 #ifdef __SPU__
 #include <spu_printf.h>
 #define printf spu_printf
-//#define DEBUG_SPU_COLLISION_DETECTION 1
 #endif //__SPU__
 #endif
 
@@ -81,17 +80,18 @@ void        btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 #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)
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
 #endif
 {
        m_cachedSeparatingDistance = 0.f;
 
        btScalar distance=btScalar(0.);
        btVector3       normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+
        btVector3 pointOnA,pointOnB;
        btTransform     localTransA = input.m_transformA;
        btTransform localTransB = input.m_transformB;
-       btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+       btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
        localTransA.getOrigin() -= positionOffset;
        localTransB.getOrigin() -= positionOffset;
 
@@ -102,17 +102,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
        gNumGjkChecks++;
 
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-       spu_printf("inside gjk\n");
-#endif
        //for CCD we don't use margins
        if (m_ignoreMargin)
        {
                marginA = btScalar(0.);
                marginB = btScalar(0.);
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-               spu_printf("ignoring margin\n");
-#endif
        }
 
        m_curIter = 0;
@@ -143,37 +137,13 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                        btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
                        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);
                        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-#else
-                       btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
-                       btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
-#ifdef TEST_NON_VIRTUAL
-                       btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
-                       btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
-                       btAssert((pInAv-pInA).length() < 0.0001);
-                       btAssert((qInBv-qInB).length() < 0.0001);
-#endif //
-#endif //__SPU__
-#endif
-
 
                        btVector3  pWorld = localTransA(pInA);  
                        btVector3  qWorld = localTransB(qInB);
 
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-               spu_printf("got local supporting vertices\n");
-#endif
 
                        if (check2d)
                        {
@@ -217,14 +187,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                                break;
                        }
 
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-               spu_printf("addVertex 1\n");
-#endif
                        //add current vertex to simplex
                        m_simplexSolver->addVertex(w, pWorld, qWorld);
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-               spu_printf("addVertex 2\n");
-#endif
                        btVector3 newCachedSeparatingAxis;
 
                        //calculate the closest point to the origin (update vector v)
@@ -274,7 +238,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                          //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
               if (m_curIter++ > gGjkMaxIter)   
               {   
-                      #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
+                      #if defined(DEBUG) || defined (_DEBUG)
 
                               printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
@@ -307,6 +271,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                {
                        m_simplexSolver->compute_points(pointOnA, pointOnB);
                        normalInB = m_cachedSeparatingAxis;
+
                        btScalar lenSqr =m_cachedSeparatingAxis.length2();
                        
                        //valid normal
@@ -318,6 +283,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                        {
                                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
                                normalInB *= rlen; //normalize
+
                                btScalar s = btSqrt(squaredDistance);
                        
                                btAssert(s > btScalar(0.0));
@@ -373,6 +339,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                                        {
                                                tmpNormalInB /= btSqrt(lenSqr);
                                                btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+                                               m_lastUsedMethod = 3;
                                                //only replace valid penetrations when the result is deeper (check)
                                                if (!isValid || (distance2 < distance))
                                                {
@@ -380,8 +347,48 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                                                        pointOnA = tmpPointOnA;
                                                        pointOnB = tmpPointOnB;
                                                        normalInB = tmpNormalInB;
+                                                       ///todo: need to track down this EPA penetration solver degeneracy
+                                                       ///the penetration solver reports penetration but the contact normal
+                                                       ///connecting the contact points is pointing in the opposite direction
+                                                       ///until then, detect the issue and revert the normal
+                                                       {
+                                                               btScalar d1=0;
+                                                               {
+                                                                       btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
+                                                                       btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
+                                                               
+
+                                                                       btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+                                                                       btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+                                                                       btVector3  pWorld = localTransA(pInA);  
+                                                                       btVector3  qWorld = localTransB(qInB);
+                                                                       btVector3 w     = pWorld - qWorld;
+                                                                       d1 = (-normalInB).dot(w);
+                                                               }
+                                                               btScalar d0 = 0.f;
+                                                               {
+                                                                       btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
+                                                                       btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
+                                                               
+
+                                                                       btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+                                                                       btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+                                                                       btVector3  pWorld = localTransA(pInA);  
+                                                                       btVector3  qWorld = localTransB(qInB);
+                                                                       btVector3 w     = pWorld - qWorld;
+                                                                       d0 = normalInB.dot(w);
+                                                               }
+                                                               if (d1>d0)
+                                                               {
+                                                                       m_lastUsedMethod = 10;
+                                                                       normalInB*=-1;
+                                                               } 
+
+                                                       }
                                                        isValid = true;
-                                                       m_lastUsedMethod = 3;
+                                                       
                                                } else
                                                {
                                                        m_lastUsedMethod = 8;
@@ -413,6 +420,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                                                        pointOnB += m_cachedSeparatingAxis * marginB ;
                                                        normalInB = m_cachedSeparatingAxis;
                                                        normalInB.normalize();
+
                                                        isValid = true;
                                                        m_lastUsedMethod = 6;
                                                } else
@@ -431,36 +439,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
        if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
        {
-#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 
 
-               if (m_fixContactNormalDirection)
-               {
-                       ///@workaround for sticky convex collisions
-                       //in some degenerate cases (usually when the use uses very small margins) 
-                       //the contact normal is pointing the wrong direction
-                       //so fix it now (until we can deal with all degenerate cases in GJK and EPA)
-                       //contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A
-                       //We like to use a dot product of the normal against the difference of the centroids, 
-                       //once the centroid is available in the API
-                       //until then we use the center of the aabb to approximate the centroid
-                       btVector3 aabbMin,aabbMax;
-                       m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax);
-                       btVector3 posA  = (aabbMax+aabbMin)*btScalar(0.5);
-               
-                       m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax);
-                       btVector3 posB = (aabbMin+aabbMax)*btScalar(0.5);
-
-                       btVector3 diff = posA-posB;
-                       if (diff.dot(normalInB) < 0.f)
-                               normalInB *= -1.f;
-               }
                m_cachedSeparatingAxis = normalInB;
                m_cachedSeparatingDistance = distance;