/// <summary>
/// Finds the incident edge using the specified c
/// </summary>
/// <param name="c">The </param>
/// <param name="poly1">The poly</param>
/// <param name="xf1">The xf</param>
/// <param name="edge1">The edge</param>
/// <param name="poly2">The poly</param>
/// <param name="xf2">The xf</param>
public static void FindIncidentEdge(out ClipVertex[] c,
PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vec2[] vertices2 = poly2.Vertices;
Vec2[] normals2 = poly2.Normals;
Box2DxDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vec2 normal1 = Math.MulT(xf2.R, Math.Mul(xf1.R, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FltMax;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.Dot(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 = new ClipVertex[2];
c[0].V = Math.Mul(xf2, vertices2[i1]);
c[0].Id.Features.ReferenceEdge = (byte)edge1;
c[0].Id.Features.IncidentEdge = (byte)i1;
c[0].Id.Features.IncidentVertex = 0;
c[1].V = Math.Mul(xf2, vertices2[i2]);
c[1].Id.Features.ReferenceEdge = (byte)edge1;
c[1].Id.Features.IncidentEdge = (byte)i2;
c[1].Id.Features.IncidentVertex = 1;
}
// Sutherland-Hodgman clipping.
/// <summary>
/// Clips the segment to line using the specified v out
/// </summary>
/// <param name="vOut">The out</param>
/// <param name="vIn">The in</param>
/// <param name="normal">The normal</param>
/// <param name="offset">The offset</param>
/// <returns>The num out</returns>
public static int ClipSegmentToLine(out ClipVertex[] /*2*/ vOut, ClipVertex[] /*2*/ vIn, Vec2 normal,
float offset)
{
vOut = new ClipVertex[2];
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vec2.Dot(normal, vIn[0].V) - offset;
float distance1 = Vec2.Dot(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);
if (distance0 > 0.0f)
{
vOut[numOut].Id = vIn[0].Id;
}
else
{
vOut[numOut].Id = vIn[1].Id;
}
++numOut;
}
return(numOut);
}