/// Clipping for contact manifolds.
public static int b2ClipSegmentToLine(b2ClipVertex[] vOut, b2ClipVertex[] vIn,
b2Vec2 normal, float offset, byte vertexIndexA)
{
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = b2Math.b2Dot(normal, vIn[0].v) - offset;
float distance1 = b2Math.b2Dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
// VertexA is hitting edgeB.
vOut[numOut].id.indexA = vertexIndexA;
vOut[numOut].id.indexB = vIn[0].id.indexB;
vOut[numOut].id.typeA = b2ContactFeatureType.e_vertex;
vOut[numOut].id.typeB = b2ContactFeatureType.e_face;
++numOut;
}
return numOut;
}
public static void b2FindIncidentEdge(b2ClipVertex[] c,
b2PolygonShape poly1, b2Transform xf1, int edge1,
b2PolygonShape poly2, b2Transform xf2)
{
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
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;
}
/// Compute the collision manifold between two polygons.
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void b2CollidePolygons(ref b2Manifold manifold,
b2PolygonShape polyA, ref b2Transform xfA,
b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
return;
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.type = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = new b2ClipVertex[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = localTangent.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent);
b2Vec2 normal = tangent.UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
v11 = b2Math.b2Mul(xf1, v11);
v12 = b2Math.b2Mul(xf1, v12);
// Face offset.
float frontOffset = b2Math.b2Dot(ref normal, ref v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -b2Math.b2Dot(ref tangent, ref v11) + totalRadius;
float sideOffset2 = b2Math.b2Dot(ref tangent, ref v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation = b2Math.b2Dot(ref normal, ref clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public static void b2FindIncidentEdge(b2ClipVertex[] c,
b2PolygonShape poly1, ref b2Transform xf1, int edge1,
b2PolygonShape poly2, ref b2Transform xf2)
{
#if false
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
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(ref normal1, ref 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(ref xf2, ref 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(ref xf2, ref 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;
#else
var edge = poly1.Normals[edge1];
int count2 = poly2.VertexCount;
b2Vec2[] vertices2 = poly2.Vertices;
b2Vec2[] normals2 = poly2.Normals;
// Get the normal of the reference edge in poly2's frame.
float bx = xf1.q.c * edge.x - xf1.q.s * edge.y;
float by = xf1.q.s * edge.x + xf1.q.c * edge.y;
float normal1x = xf2.q.c * bx + xf2.q.s * by;
float normal1y = -xf2.q.s * bx + xf2.q.c * by;
// Find the incident edge on poly2.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
var normal = normals2[i];
float dot = normal1x * normal.x + normal1y * normal.y;
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;
b2ClipVertex vertex;
var vi1 = vertices2[i1];
var vi2 = vertices2[i2];
vertex.v.x = (xf2.q.c * vi1.x - xf2.q.s * vi1.y) + xf2.p.x;
vertex.v.y = (xf2.q.s * vi1.x + xf2.q.c * vi1.y) + xf2.p.y;
vertex.id.indexA = (byte)edge1;
vertex.id.indexB = (byte)i1;
vertex.id.typeA = b2ContactFeatureType.e_face;
vertex.id.typeB = b2ContactFeatureType.e_vertex;
c[0] = vertex;
vertex.v.x = (xf2.q.c * vi2.x - xf2.q.s * vi2.y) + xf2.p.x;
vertex.v.y = (xf2.q.s * vi2.x + xf2.q.c * vi2.y) + xf2.p.y;
vertex.id.indexA = (byte)edge1;
vertex.id.indexB = (byte)i2;
vertex.id.typeA = b2ContactFeatureType.e_face;
vertex.id.typeB = b2ContactFeatureType.e_vertex;
c[1] = vertex;
#endif
}
/// Clipping for contact manifolds.
public static int b2ClipSegmentToLine(b2ClipVertex[] vOut, b2ClipVertex[] vIn,
ref b2Vec2 normal, float offset, byte vertexIndexA)
{
// Start with no output points
int numOut = 0;
var v0 = vIn[0].v;
var v1 = vIn[1].v;
// Calculate the distance of end points to the line
float distance0 = normal.x * v0.x + normal.y * v0.y - offset;
float distance1 = normal.x * v1.x + normal.y * v1.y - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
b2ClipVertex o;
o.v.x = v0.x + interp * (v1.x - v0.x);
o.v.y = v0.y + interp * (v1.y - v0.y);
// VertexA is hitting edgeB.
o.id.indexA = vertexIndexA;
o.id.indexB = vIn[0].id.indexB;
o.id.typeA = b2ContactFeatureType.e_vertex;
o.id.typeB = b2ContactFeatureType.e_face;
vOut[numOut] = o;
++numOut;
}
return numOut;
}
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
public void Collide(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA,
b2PolygonShape polygonB, ref b2Transform xfB)
{
m_xf = b2Math.b2MulT(xfA, xfB);
m_centroidB = b2Math.b2Mul(m_xf, polygonB.Centroid);
m_v0 = edgeA.Vertex0;
m_v1 = edgeA.Vertex1;
m_v2 = edgeA.Vertex2;
m_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
float offset1 = b2Math.b2Dot(m_normal1, m_centroidB - m_v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = b2Math.b2Cross(edge0, edge1) >= 0.0f;
offset0 = b2Math.b2Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = b2Math.b2Cross(edge1, edge2) > 0.0f;
offset2 = b2Math.b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB = b2TempPolygon.Create();
m_polygonB.count = polygonB.VertexCount;
for (int i = 0; i < polygonB.VertexCount; ++i)
{
m_polygonB.vertices[i] = b2Math.b2Mul(m_xf, polygonB.Vertices[i]);
m_polygonB.normals[i] = b2Math.b2Mul(m_xf.q, polygonB.Normals[i]);
}
m_radius = 2.0f * b2Settings.b2_polygonRadius;
manifold.pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// Console.WriteLine("b2EPAxis: {0} {1} {2}", edgeAxis.index, edgeAxis.separation, edgeAxis.type);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxisType.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxisType.e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxisType.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex[] ie = new b2ClipVertex[2];
b2ReferenceFace rf;
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
manifold.type = b2ManifoldType.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = b2Math.b2Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = b2Math.b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)i1;
ie[0].id.typeA = b2ContactFeatureType.e_face;
ie[0].id.typeB = b2ContactFeatureType.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)i2;
ie[1].id.typeA = b2ContactFeatureType.e_face;
ie[1].id.typeB = b2ContactFeatureType.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = b2ManifoldType.e_faceB;
ie[0].v = m_v1;
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)primaryAxis.index;
ie[0].id.typeA = b2ContactFeatureType.e_vertex;
ie[0].id.typeB = b2ContactFeatureType.e_face;
ie[1].v = m_v2;
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)primaryAxis.index;
ie[1].id.typeA = b2ContactFeatureType.e_vertex;
ie[1].id.typeB = b2ContactFeatureType.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1 = new b2Vec2(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = b2Math.b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = b2Math.b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, (byte)rf.i1);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, (byte)rf.i2);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
manifold.localNormal = rf.normal;
manifold.localPoint = rf.v1;
}
else
{
manifold.localNormal = polygonB.Normals[rf.i1];
manifold.localPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation;
separation = b2Math.b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
cp.localPoint = b2Math.b2MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.typeA = clipPoints2[i].id.typeB;
cp.id.typeB = clipPoints2[i].id.typeA;
cp.id.indexA = clipPoints2[i].id.indexB;
cp.id.indexB = clipPoints2[i].id.indexA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
public void Collide(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA,
b2PolygonShape polygonB, ref b2Transform xfB)
{
m_xf = b2Math.b2MulT(xfA, xfB);
m_centroidB = b2Math.b2Mul(m_xf, polygonB.Centroid);
m_v0 = edgeA.Vertex0;
m_v1 = edgeA.Vertex1;
m_v2 = edgeA.Vertex2;
m_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
b2Vec2 cenMinusV1 = m_centroidB - m_v1;
float offset1 = b2Math.b2Dot(ref m_normal1, ref cenMinusV1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = b2Math.b2Cross(ref edge0, ref edge1) >= 0.0f;
b2Vec2 cenMinusV0 = m_centroidB - m_v0;
offset0 = b2Math.b2Dot(ref m_normal0, ref cenMinusV0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = b2Math.b2Cross(edge1, edge2) > 0.0f;
offset2 = b2Math.b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB = b2TempPolygon.Create();
m_polygonB.count = polygonB.VertexCount;
for (int i = 0; i < polygonB.VertexCount; ++i)
{
m_polygonB.vertices[i] = b2Math.b2Mul(m_xf, polygonB.Vertices[i]);
m_polygonB.normals[i] = b2Math.b2Mul(m_xf.q, polygonB.Normals[i]);
}
m_radius = 2.0f * b2Settings.b2_polygonRadius;
manifold.pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// Console.WriteLine("b2EPAxis: {0} {1} {2}", edgeAxis.index, edgeAxis.separation, edgeAxis.type);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxisType.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxisType.e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxisType.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex[] ie = new b2ClipVertex[2];
b2ReferenceFace rf;
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
manifold.type = b2ManifoldType.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = b2Math.b2Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = b2Math.b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)i1;
ie[0].id.typeA = b2ContactFeatureType.e_face;
ie[0].id.typeB = b2ContactFeatureType.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)i2;
ie[1].id.typeA = b2ContactFeatureType.e_face;
ie[1].id.typeB = b2ContactFeatureType.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = b2ManifoldType.e_faceB;
ie[0].v = m_v1;
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)primaryAxis.index;
ie[0].id.typeA = b2ContactFeatureType.e_vertex;
ie[0].id.typeB = b2ContactFeatureType.e_face;
ie[1].v = m_v2;
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)primaryAxis.index;
ie[1].id.typeA = b2ContactFeatureType.e_vertex;
ie[1].id.typeB = b2ContactFeatureType.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1 = new b2Vec2(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = b2Math.b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = b2Math.b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, (byte)rf.i1);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, (byte)rf.i2);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
manifold.localNormal = rf.normal;
manifold.localPoint = rf.v1;
}
else
{
manifold.localNormal = polygonB.Normals[rf.i1];
manifold.localPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation;
separation = b2Math.b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
cp.localPoint = b2Math.b2MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.typeA = clipPoints2[i].id.typeB;
cp.id.typeB = clipPoints2[i].id.typeA;
cp.id.indexA = clipPoints2[i].id.indexB;
cp.id.indexB = clipPoints2[i].id.indexA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}