public void Collide(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
Transform.MulTransToOutUnsafe(xfA, xfB, xf);
Transform.MulToOutUnsafe(xf, polygonB.Centroid, centroidB);
v0 = edgeA.Vertex0;
v1 = edgeA.Vertex1;
v2 = edgeA.Vertex2;
v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
edge1.Set(v2).SubLocal(v1);
edge1.Normalize();
normal1.Set(edge1.Y, -edge1.X);
float offset1 = Vec2.Dot(normal1, temp.Set(centroidB).SubLocal(v1));
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
edge0.Set(v1).SubLocal(v0);
edge0.Normalize();
normal0.Set(edge0.Y, -edge0.X);
convex1 = Vec2.Cross(edge0, edge1) >= 0.0f;
offset0 = Vec2.Dot(normal0, temp.Set(centroidB).SubLocal(v0));
}
// Is there a following edge?
if (hasVertex3)
{
edge2.Set(v3).SubLocal(v2);
edge2.Normalize();
normal2.Set(edge2.Y, -edge2.X);
convex2 = Vec2.Cross(edge1, edge2) > 0.0f;
offset2 = Vec2.Dot(normal2, temp.Set(centroidB).SubLocal(v2));
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal0);
upperLimit.Set(normal2);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1).NegateLocal();
}
}
else if (convex1)
{
front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal0);
upperLimit.Set(normal1);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal2).NegateLocal();
upperLimit.Set(normal1).NegateLocal();
}
}
else if (convex2)
{
front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1);
upperLimit.Set(normal2);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal0).NegateLocal();
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1);
upperLimit.Set(normal1);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal2).NegateLocal();
upperLimit.Set(normal0).NegateLocal();
}
}
}
else if (hasVertex0)
{
if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal0);
upperLimit.Set(normal1).NegateLocal();
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1);
upperLimit.Set(normal1).NegateLocal();
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1);
upperLimit.Set(normal1).NegateLocal();
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1);
upperLimit.Set(normal0).NegateLocal();
}
}
}
else if (hasVertex3)
{
if (convex2)
{
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal2);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1);
}
}
else
{
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal2).NegateLocal();
upperLimit.Set(normal1);
}
}
}
else
{
front = offset1 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1).NegateLocal();
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1);
upperLimit.Set(normal1);
}
}
// Get polygonB in frameA
this.polygonB.Count = polygonB.VertexCount;
for (int i = 0; i < polygonB.VertexCount; ++i)
{
Transform.MulToOutUnsafe(xf, polygonB.Vertices[i], this.polygonB.Vertices[i]);
Rot.MulToOutUnsafe(xf.Q, polygonB.Normals[i], this.polygonB.Normals[i]);
}
radius = 2.0f * Settings.POLYGON_RADIUS;
manifold.PointCount = 0;
ComputeEdgeSeparation(edgeAxis);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxis.EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > radius)
{
return;
}
ComputePolygonSeparation(polygonAxis);
if (polygonAxis.Type != EPAxis.EPAxisType.Unknown && polygonAxis.Separation > radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.Type == EPAxis.EPAxisType.Unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
// ClipVertex[] ie = new ClipVertex[2];
if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA)
{
manifold.Type = Manifold.ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vec2.Dot(normal, this.polygonB.Normals[0]);
for (int i = 1; i < this.polygonB.Count; ++i)
{
float value = Vec2.Dot(normal, this.polygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < this.polygonB.Count ? i1 + 1 : 0;
ie[0].V.Set(this.polygonB.Vertices[i1]);
ie[0].Id.IndexA = 0;
ie[0].Id.IndexB = (sbyte)i1;
ie[0].Id.TypeA = (sbyte)ContactID.Type.Face;
ie[0].Id.TypeB = (sbyte)ContactID.Type.Vertex;
ie[1].V.Set(this.polygonB.Vertices[i2]);
ie[1].Id.IndexA = 0;
ie[1].Id.IndexB = (sbyte)i2;
ie[1].Id.TypeA = (sbyte)ContactID.Type.Face;
ie[1].Id.TypeB = (sbyte)ContactID.Type.Vertex;
if (front)
{
rf.I1 = 0;
rf.I2 = 1;
rf.V1.Set(v1);
rf.V2.Set(v2);
rf.Normal.Set(normal1);
}
else
{
rf.I1 = 1;
rf.I2 = 0;
rf.V1.Set(v2);
rf.V2.Set(v1);
rf.Normal.Set(normal1).NegateLocal();
}
}
else
{
manifold.Type = Manifold.ManifoldType.FaceB;
ie[0].V.Set(v1);
ie[0].Id.IndexA = 0;
ie[0].Id.IndexB = (sbyte)primaryAxis.Index;
ie[0].Id.TypeA = (sbyte)ContactID.Type.Vertex;
ie[0].Id.TypeB = (sbyte)ContactID.Type.Face;
ie[1].V.Set(v2);
ie[1].Id.IndexA = 0;
ie[1].Id.IndexB = (sbyte)primaryAxis.Index;
ie[1].Id.TypeA = (sbyte)ContactID.Type.Vertex;
ie[1].Id.TypeB = (sbyte)ContactID.Type.Face;
rf.I1 = primaryAxis.Index;
rf.I2 = rf.I1 + 1 < this.polygonB.Count ? rf.I1 + 1 : 0;
rf.V1.Set(this.polygonB.Vertices[rf.I1]);
rf.V2.Set(this.polygonB.Vertices[rf.I2]);
rf.Normal.Set(this.polygonB.Normals[rf.I1]);
}
rf.SideNormal1.Set(rf.Normal.Y, -rf.Normal.X);
rf.SideNormal2.Set(rf.SideNormal1).NegateLocal();
rf.SideOffset1 = Vec2.Dot(rf.SideNormal1, rf.V1);
rf.SideOffset2 = Vec2.Dot(rf.SideNormal2, rf.V2);
// Clip incident edge against extruded edge1 side edges.
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, ie, rf.SideNormal1, rf.SideOffset1, rf.I1);
if (np < Settings.MAX_MANIFOLD_POINTS)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.I2);
if (np < Settings.MAX_MANIFOLD_POINTS)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA)
{
manifold.LocalNormal.Set(rf.Normal);
manifold.LocalPoint.Set(rf.V1);
}
else
{
manifold.LocalNormal.Set(polygonB.Normals[rf.I1]);
manifold.LocalPoint.Set(polygonB.Vertices[rf.I1]);
}
int pointCount = 0;
for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; ++i)
{
float separation = Vec2.Dot(rf.Normal, temp.Set(clipPoints2[i].V).SubLocal(rf.V1));
if (separation <= radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA)
{
// cp.localPoint = MulT(m_xf, clipPoints2[i].v);
Transform.MulTransToOutUnsafe(xf, clipPoints2[i].V, cp.LocalPoint);
cp.Id.Set(clipPoints2[i].Id);
}
else
{
cp.LocalPoint.Set(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;
}
++pointCount;
}
}
manifold.PointCount = pointCount;
}
