float32 DistanceGeneric(b2Vec2* x1, b2Vec2* x2,
const T1* shape1, const b2XForm& xf1,
const T2* shape2, const b2XForm& xf2)
{
b2Vec2 p1s[3], p2s[3];
b2Vec2 points[3];
int32 pointCount = 0;
*x1 = shape1->GetFirstVertex(xf1);
*x2 = shape2->GetFirstVertex(xf2);
float32 vSqr = 0.0f;
const int32 maxIterations = 20;
for (int32 iter = 0; iter < maxIterations; ++iter)
{
b2Vec2 v = *x2 - *x1;
b2Vec2 w1 = shape1->Support(xf1, v);
b2Vec2 w2 = shape2->Support(xf2, -v);
vSqr = b2Dot(v, v);
b2Vec2 w = w2 - w1;
float32 vw = b2Dot(v, w);
if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
{
if (pointCount == 0)
{
*x1 = w1;
*x2 = w2;
}
g_GJK_Iterations = iter;
return b2Sqrt(vSqr);
}
switch (pointCount)
{
case 0:
p1s[0] = w1;
p2s[0] = w2;
points[0] = w;
*x1 = p1s[0];
*x2 = p2s[0];
++pointCount;
break;
case 1:
p1s[1] = w1;
p2s[1] = w2;
points[1] = w;
pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
break;
case 2:
p1s[2] = w1;
p2s[2] = w2;
points[2] = w;
pointCount = ProcessThree(x1, x2, p1s, p2s, points);
break;
}
// If we have three points, then the origin is in the corresponding triangle.
if (pointCount == 3)
{
g_GJK_Iterations = iter;
return 0.0f;
}
float32 maxSqr = -B2_FLT_MAX;
for (int32 i = 0; i < pointCount; ++i)
{
maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
}
#ifdef TARGET_FLOAT32_IS_FIXED
if (pointCount == 3 || vSqr <= 5.0*B2_FLT_EPSILON * maxSqr)
#else
if (vSqr <= 100.0f * B2_FLT_EPSILON * maxSqr)
#endif
{
g_GJK_Iterations = iter;
v = *x2 - *x1;
vSqr = b2Dot(v, v);
return b2Sqrt(vSqr);
}
}
g_GJK_Iterations = maxIterations;
return b2Sqrt(vSqr);
}
float32 b2Distance(b2Vec2* p1Out, b2Vec2* p2Out, const b2Shape* shape1, const b2Shape* shape2)
{
b2Vec2 p1s[3], p2s[3];
b2Vec2 points[3];
int32 pointCount = 0;
*p1Out = shape1->m_position;
*p2Out = shape2->m_position;
float32 vSqr = 0.0f;
const int32 maxIterations = 20;
for (int32 iter = 0; iter < maxIterations; ++iter)
{
b2Vec2 v = *p2Out - *p1Out;
b2Vec2 w1 = shape1->Support(v);
b2Vec2 w2 = shape2->Support(-v);
vSqr = b2Dot(v, v);
b2Vec2 w = w2 - w1;
float32 vw = b2Dot(v, w);
if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
{
if (pointCount == 0)
{
*p1Out = w1;
*p2Out = w2;
}
g_GJK_Iterations = iter;
return sqrtf(vSqr);
}
switch (pointCount)
{
case 0:
p1s[0] = w1;
p2s[0] = w2;
points[0] = w;
*p1Out = p1s[0];
*p2Out = p2s[0];
++pointCount;
break;
case 1:
p1s[1] = w1;
p2s[1] = w2;
points[1] = w;
pointCount = ProcessTwo(p1Out, p2Out, p1s, p2s, points);
break;
case 2:
p1s[2] = w1;
p2s[2] = w2;
points[2] = w;
pointCount = ProcessThree(p1Out, p2Out, p1s, p2s, points);
break;
}
// If we have three points, then the origin is in the corresponding triangle.
if (pointCount == 3)
{
g_GJK_Iterations = iter;
return 0.0f;
}
float32 maxSqr = -FLT_MAX;
for (int32 i = 0; i < pointCount; ++i)
{
maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
}
if (pointCount == 3 || vSqr <= 100.0f * FLT_EPSILON * maxSqr)
{
g_GJK_Iterations = iter;
return sqrtf(vSqr);
}
}
g_GJK_Iterations = maxIterations;
return sqrtf(vSqr);
}