Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Work on fixing some GJK issues reported by Pierre Terdiman (thanks Pierre for the testbed!)

Browse Source 
Improved this penetration test with more verbose output
retrieve worldtransform from motionstate when rigidbody gets motionstate assigned
This commit is contained in:
ejcoumans
2006-11-11 23:59:51 +00:00
parent 82132b7b5f
commit 86c27a7c9d
10 changed files with 331 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
View File

@@ -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;
}
}
}
}

8
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
View File

@@ -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

56
src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
View File

@@ -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;

2
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -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