// Find the max separation between poly1 and poly2 using edge normals from poly1.
static btScalar FindMaxSeparation(int* edgeIndex,
const btBox2dShape* poly1, const btTransform& xf1,
const btBox2dShape* poly2, const btTransform& xf2)
{
int count1 = poly1->getVertexCount();
const btVector3* normals1 = poly1->getNormals();
// Vector pointing from the centroid of poly1 to the centroid of poly2.
btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
btScalar maxDot;
if( count1 > 0 )
edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
// Get the separation for the edge normal.
btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
if (sPrev > 0.0f)
{
return sPrev;
}
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
if (sNext > 0.0f)
{
return sNext;
}
// Find the best edge and the search direction.
int bestEdge;
btScalar bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
*edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for ( ; ; )
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
*edgeIndex = bestEdge;
return bestSeparation;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float32 FindMaxSeparation(int32* edgeIndex,
const b2PolygonShape* poly1, const b2XForm& xf1,
const b2PolygonShape* poly2, const b2XForm& xf2)
{
int32 count1 = poly1->GetVertexCount();
const b2Vec2* normals1 = poly1->GetNormals();
// Vector pointing from the centroid of poly1 to the centroid of poly2.
b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid());
b2Vec2 dLocal1 = b2MulT(xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int32 edge = 0;
float32 maxDot = -B2_FLT_MAX;
for (int32 i = 0; i < count1; ++i)
{
float32 dot = b2Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
// Check the separation for the previous edge normal.
int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float32 sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
if (sPrev > 0.0f)
{
return sPrev;
}
// Check the separation for the next edge normal.
int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float32 sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
if (sNext > 0.0f)
{
return sNext;
}
// Find the best edge and the search direction.
int32 bestEdge;
float32 bestSeparation;
int32 increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
*edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for ( ; ; )
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
*edgeIndex = bestEdge;
return bestSeparation;
}
