static void FindIncidentEdge(ClipVertex[] c,
Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
IndexedVector3[] normals2 = poly2.GetNormals();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Get the normal of the reference edge in poly2's frame.
IndexedVector3 normal1 = xf2._basis.Transpose() * (xf1._basis * normals1[edge1]);
// Find the incident edge on poly2.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = normal1.Dot(normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = xf2 * vertices2[i1];
// c[0].id.features.referenceEdge = (unsigned char)edge1;
// c[0].id.features.incidentEdge = (unsigned char)i1;
// c[0].id.features.incidentVertex = 0;
c[1].v = xf2 * vertices2[i2];
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
}
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static float EdgeSeparation(Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] vertices1 = poly1.GetVertices();
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Convert normal from poly1's frame into poly2's frame.
IndexedVector3 normal1World = xf1._basis * normals1[edge1];
IndexedVector3 normal1 = xf1._basis.Transpose() * normal1World;
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = vertices2[i].Dot(normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
IndexedVector3 v1 = xf1 * vertices1[edge1];
IndexedVector3 v2 = xf2 * vertices2[index];
float separation = (v2 - v1).Dot(normal1World);
return(separation);
}
static void FindIncidentEdge(ClipVertex[] c,
Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
IndexedVector3[] normals2 = poly2.GetNormals();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Get the normal of the reference edge in poly2's frame.
IndexedVector3 normal1 = xf2._basis.Transpose() * (xf1._basis * normals1[edge1]);
// Find the incident edge on poly2.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = normal1.Dot(normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = xf2 * vertices2[i1];
// c[0].id.features.referenceEdge = (unsigned char)edge1;
// c[0].id.features.incidentEdge = (unsigned char)i1;
// c[0].id.features.incidentVertex = 0;
c[1].v = xf2 * vertices2[i2];
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
}
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static float EdgeSeparation(Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] vertices1 = poly1.GetVertices();
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Convert normal from poly1's frame into poly2's frame.
IndexedVector3 normal1World = xf1._basis * normals1[edge1];
IndexedVector3 normal1 = xf1._basis.Transpose() * normal1World;
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = vertices2[i].Dot(normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
IndexedVector3 v1 = xf1 * vertices1[edge1];
IndexedVector3 v2 = xf2 * vertices2[index];
float separation = (v2 - v1).Dot(normal1World);
return separation;
}