public void getClosestPoints(ref ClosestPointInput input, ref PerturbedContactResult output, ref ManifoldResult originalManifoldResult, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
btVector3 normalInB = new btVector3(0f, 0f, 0f);
btVector3 pointOnA, pointOnB = btVector3.Zero;
btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB;
btVector3 positionOffset;// = (localTransA.Origin + localTransB.Origin) * 0.5f;
{
btVector3 temp;
btVector3.Add(ref localTransA.Origin, ref localTransB.Origin, out temp);
btVector3.Multiply(ref temp, 0.5f, out positionOffset);
}
localTransA.Origin -= positionOffset;
localTransB.Origin -= positionOffset;
bool check2d = m_minkowskiA.isConvex2d && m_minkowskiB.isConvex2d;
float marginA = m_marginA;
float marginB = m_marginB;
//gNumGjkChecks++;
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = 0f;
marginB = 0f;
}
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 = 0;
m_lastUsedMethod = -1;
{
float squaredDistance = BulletGlobal.BT_LARGE_FLOAT;
float delta = 0f;
float margin = marginA + marginB;
m_simplexSolver.reset();
for (; ; )
//while (true)
{
btVector3 seperatingAxisInA;// = (-m_cachedSeparatingAxis) * input.m_transformA.Basis;
{
btVector3 temp = -m_cachedSeparatingAxis;
btMatrix3x3.Multiply(ref temp, ref input.m_transformA.Basis, out seperatingAxisInA);
}
btVector3 seperatingAxisInB;// = m_cachedSeparatingAxis * input.m_transformB.Basis;
btMatrix3x3.Multiply(ref m_cachedSeparatingAxis, ref input.m_transformB.Basis, out seperatingAxisInB);
btVector3 pInA;// = m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA, out pInA);
btVector3 qInB;// = m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB, out qInB);
btVector3 pWorld = btVector3.Transform(pInA, localTransA);
btVector3 qWorld = btVector3.Transform(qInB, localTransB);
if (check2d)
{
pWorld.Z = 0f;
qWorld.Z = 0f;
}
btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap
if ((delta > 0.0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if (m_simplexSolver.inSimplex(w))
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
float f0 = squaredDistance - delta;
float f1 = squaredDistance * REL_ERROR2;
if (f0 <= f1)
{
if (f0 <= 0f)
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.addVertex(w, pWorld, qWorld);
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if (!m_simplexSolver.closest(out newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if (newCachedSeparatingAxis.Length2 < REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
float previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.Length2;
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= BulletGlobal.SIMD_EPSILON * previousSquaredDistance)
{
m_simplexSolver.backup_closest(ref m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
//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)
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(),
m_cachedSeparatingAxis.getZ(),
squaredDistance,
m_minkowskiA->getShapeType(),
m_minkowskiB->getShapeType());
#endif */
break;
}
bool check = (!m_simplexSolver.fullSimplex);
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
m_simplexSolver.backup_closest(ref m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver.compute_points(out pointOnA, out pointOnB);
normalInB = pointOnA - pointOnB;
float lenSqr = m_cachedSeparatingAxis.Length2;
//valid normal
if (lenSqr < 0.0001)
{
m_degenerateSimplex = 5;
}
if (lenSqr > BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON)
{
float rlen = 1f / (float)Math.Sqrt(lenSqr);
normalInB *= rlen; //normalize
float s = (float)Math.Sqrt(squaredDistance);
Debug.Assert(s > 0.0f);
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1f / rlen) - margin);
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies != 0 && m_penetrationDepthSolver != null && m_degenerateSimplex != 0 && ((distance + margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
//penetration case
//if there is no way to handle penetrations, bail out
if (m_penetrationDepthSolver != null)
{
// Penetration depth case.
btVector3 tmpPointOnA, tmpPointOnB;
//gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis.setZero();
bool isValid2 = m_penetrationDepthSolver.calcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
localTransA, localTransB,
ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw//, input.m_stackAlloc
);
if (isValid2)
{
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lenSqr = tmpNormalInB.Length2;
if (lenSqr <= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.Length2;
}
if (lenSqr > (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON))
{
tmpNormalInB /= (float)Math.Sqrt(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
{
m_lastUsedMethod = 8;
}
}
else
{
m_lastUsedMethod = 9;
}
}
else
{
///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 > 0f)
{
float 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 && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
output.addContactPoint(
normalInB,
pointOnB + positionOffset,
distance, ref originalManifoldResult);
}
}
