public static int ClipSegmentToLine(ClipVertex[] vOut, ClipVertex[] vIn,
Vector3 normal, float offset)
{
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vector3.Dot(normal, vIn[0].v) - offset;
float distance1 = Vector3.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;
}
static void FindIncidentEdge(ClipVertex[] c,
Box2dShape poly1, ref Matrix xf1, int edge1,
Box2dShape poly2, ref Matrix xf2)
{
Vector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
Vector3[] vertices2 = poly2.GetVertices();
Vector3[] normals2 = poly2.GetNormals();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Get the normal of the reference edge in poly2's frame.
Vector3 normal1 = Vector3.TransformNormal(Vector3.TransformNormal(normals1[edge1],xf1),MathUtil.TransposeBasis(xf2));
// Find the incident edge on poly2.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = Vector3.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[0].v = Vector3.Transform(vertices2[i1],xf2);
// 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 = Vector3.Transform(vertices2[i2],xf2);
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
}
void B2CollidePolygons(ref ManifoldResult manifold,
Box2dShape polyA, ref Matrix xfA,
Box2dShape polyB, ref Matrix xfB)
{
int edgeA = 0;
float separationA = FindMaxSeparation(ref edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > 0.0f)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(ref edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > 0.0f)
{
return;
}
Box2dShape poly1; // reference poly
Box2dShape poly2; // incident poly
Matrix xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = false;
}
ClipVertex[] incidentEdge = new ClipVertex[2];
FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.GetVertexCount();
Vector3[] vertices1 = poly1.GetVertices();
Vector3 v11 = vertices1[edge1];
Vector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
Vector3 dv = v12 - v11;
Vector3 sideNormal = Vector3.TransformNormal( v12 - v11,xf1);
sideNormal.Normalize();
Vector3 frontNormal = CrossS(ref sideNormal, 1.0f);
v11 = Vector3.Transform(v11,xf1);
v12 = Vector3.Transform(v12,xf1);
float frontOffset = Vector3.Dot(frontNormal, v11);
float sideOffset1 = -Vector3.Dot(sideNormal, v11);
float sideOffset2 = Vector3.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
clipPoints1[0].v = Vector3.Zero;
clipPoints1[1].v = Vector3.Zero;
ClipVertex[] clipPoints2 = new ClipVertex[2];
clipPoints2[0].v = Vector3.Zero;
clipPoints2[1].v = Vector3.Zero;
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
Vector3 manifoldNormal = flip ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < b2_maxManifoldPoints; ++i)
{
float separation = Vector3.Dot(frontNormal, clipPoints2[i].v) - frontOffset;
if (separation <= 0.0f)
{
//b2ManifoldPoint* cp = manifold.points + pointCount;
//float separation = separation;
//cp.localPoint1 = b2MulT(xfA, clipPoints2[i].v);
//cp.localPoint2 = b2MulT(xfB, clipPoints2[i].v);
manifold.AddContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
// cp.id = clipPoints2[i].id;
// cp.id.features.flip = flip;
++pointCount;
}
}
// manifold.pointCount = pointCount;}
}