Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp')
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp119
1 files changed, 49 insertions, 70 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 8877579496b..759443a9613 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -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;
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-18 11:35:59 +0300