public static void b2FindIncidentEdge(b2ClipVertex[] c,
b2PolygonShape poly1, b2Transform xf1, int edge1,
b2PolygonShape poly2, b2Transform xf2)
{
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.GetVertexCount();
b2Vec2[] vertices2 = poly2.Vertices;
b2Vec2[] normals2 = poly2.Normals;
// Get the normal of the reference edge in poly2's frame.
b2Vec2 normal1 = b2Math.b2MulT(xf2.q, b2Math.b2Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = b2Math.b2Dot(normal1, 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 = b2Math.b2Mul(xf2, vertices2[i1]);
c[0].id.indexA = (byte)edge1;
c[0].id.indexB = (byte)i1;
c[0].id.typeA = b2ContactFeatureType.e_face;
c[0].id.typeB = b2ContactFeatureType.e_vertex;
c[1].v = b2Math.b2Mul(xf2, vertices2[i2]);
c[1].id.indexA = (byte)edge1;
c[1].id.indexB = (byte)i2;
c[1].id.typeA = b2ContactFeatureType.e_face;
c[1].id.typeB = b2ContactFeatureType.e_vertex;
}
public static float b2EdgeSeparation(b2PolygonShape poly1, b2Transform xf1, int edge1,
b2PolygonShape poly2, b2Transform xf2)
{
b2Vec2[] vertices1 = poly1.Vertices;
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.GetVertexCount();
b2Vec2[] vertices2 = poly2.Vertices;
// Convert normal from poly1's frame into poly2's frame.
b2Vec2 normal1World = b2Math.b2Mul(xf1.q, normals1[edge1]);
b2Vec2 normal1 = b2Math.b2MulT(xf2.q, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = b2Math.b2Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
b2Vec2 v1 = b2Math.b2Mul(xf1, vertices1[edge1]);
b2Vec2 v2 = b2Math.b2Mul(xf2, vertices2[index]);
float separation = b2Math.b2Dot(v2 - v1, normal1World);
return separation;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
public static float b2FindMaxSeparation(out int edgeIndex,
b2PolygonShape poly1, b2Transform xf1,
b2PolygonShape poly2, b2Transform xf2)
{
int count1 = poly1.GetVertexCount();
b2Vec2[] normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
b2Vec2 d = b2Math.b2Mul(xf2, poly2.Centroid) - b2Math.b2Mul(xf1, poly1.Centroid);
b2Vec2 dLocal1 = b2Math.b2MulT(xf1.q, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -b2Settings.b2_maxFloat;
for (int i = 0; i < count1; ++i)
{
float dot = b2Math.b2Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float 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 = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
}
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(b2Manifold manifold,
b2PolygonShape polygonA, b2Transform xfA,
b2CircleShape circleB, b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Math.b2Mul(xfB, circleB.m_p);
b2Vec2 cLocal = b2Math.b2MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.GetVertexCount();
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = b2Math.b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
return;
}
// Compute barycentric coordinates
float u1 = b2Math.b2Dot(cLocal - v1, v2 - v1);
float u2 = b2Math.b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
separation = b2Math.b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id = b2ContactFeature.Zero;
}
}
