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.cpp179
1 files changed, 136 insertions, 43 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 331d25623df..8af16b9cf6f 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -38,20 +38,49 @@ int gNumDeepPenetrationChecks = 0;
int gNumGjkChecks = 0;
-
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),
+m_catchDegeneracies(1)
+{
+}
+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 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;
@@ -64,21 +93,10 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
localTransA.getOrigin() -= positionOffset;
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++;
@@ -107,7 +125,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
m_lastUsedMethod = -1;
{
- btScalar squaredDistance = SIMD_INFINITY;
+ btScalar squaredDistance = BT_LARGE_FLOAT;
btScalar delta = btScalar(0.);
btScalar margin = marginA + marginB;
@@ -123,6 +141,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
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);
@@ -136,6 +163,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
btAssert((qInBv-qInB).length() < 0.0001);
#endif //
#endif //__SPU__
+#endif
+
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
@@ -144,12 +173,19 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
spu_printf("got local supporting vertices\n");
#endif
+ if (check2d)
+ {
+ pWorld[2] = 0.f;
+ qWorld[2] = 0.f;
+ }
+
btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
+ m_degenerateSimplex = 10;
checkSimplex=true;
//checkPenetration = false;
break;
@@ -171,6 +207,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
if (f0 <= btScalar(0.))
{
m_degenerateSimplex = 2;
+ } else
+ {
+ m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
@@ -184,34 +223,52 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
#ifdef DEBUG_SPU_COLLISION_DETECTION
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;
checkSimplex = true;
break;
}
- if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
+ if(newCachedSeparatingAxis.length2()<REL_ERROR2)
{
+ m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
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 //
+
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
{
- m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
checkSimplex = true;
+ m_degenerateSimplex = 12;
+
break;
}
+ m_cachedSeparatingAxis = newCachedSeparatingAxis;
+
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
@@ -238,7 +295,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
if (!check)
{
//do we need this backup_closest here ?
- m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+ m_degenerateSimplex = 13;
break;
}
}
@@ -246,8 +304,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
if (checkSimplex)
{
m_simplexSolver->compute_points(pointOnA, pointOnB);
- normalInB = pointOnA-pointOnB;
- btScalar lenSqr = m_cachedSeparatingAxis.length2();
+ normalInB = m_cachedSeparatingAxis;
+ btScalar lenSqr =m_cachedSeparatingAxis.length2();
+
//valid normal
if (lenSqr < 0.0001)
{
@@ -279,7 +338,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
{
//penetration case
-
+
//if there is no way to handle penetrations, bail out
if (m_penetrationDepthSolver)
{
@@ -287,19 +346,27 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
btVector3 tmpPointOnA,tmpPointOnB;
gNumDeepPenetrationChecks++;
+ m_cachedSeparatingAxis.setZero();
bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
*m_simplexSolver,
m_minkowskiA,m_minkowskiB,
localTransA,localTransB,
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
- debugDraw
+ debugDraw,input.m_stackAlloc
);
+
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))
{
tmpNormalInB /= btSqrt(lenSqr);
@@ -315,32 +382,62 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
m_lastUsedMethod = 3;
} 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)
- {
-#ifdef __SPU__
- //spu_printf("distance\n");
-#endif //__CELLOS_LV2__
+
+ 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
-#ifdef DEBUG_SPU_COLLISION_DETECTION
- spu_printf("output 1\n");
-#endif
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
@@ -349,10 +446,6 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
pointOnB+positionOffset,
distance);
-#ifdef DEBUG_SPU_COLLISION_DETECTION
- spu_printf("output 2\n");
-#endif
- //printf("gjk add:%f",distance);
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-23 09:35:57 +0300