public Range Projection(Vector2 shapePosition, Vector2 axis)
{
Polygon polygon = new Polygon(Shape);
polygon.Translate(shapePosition);
return(polygon.Projection(axis));
}
public static bool TestLineSegmentWithConvexPolygon(Vector2 startPoint, Vector2 endPoint, Polygon polygon, IEnumerable <Vector2> axes, out Contact?contact)
{
Vector2[] intersections = polygon.Intersects(new Line(startPoint, endPoint));
// no intersections, no contact
if (intersections.Length == 0)
{
contact = null;
return(false);
}
Contact leastPenetrationContact = new Contact {
Position = intersections[0],
PenetrationDepth = float.PositiveInfinity
};
// calculate line-segment first contact point position
float closestIntersectionDist = Math.DistanceSquared(startPoint, intersections[0]);
for (int i = 1; i < intersections.Length; i++)
{
float d = Math.DistanceSquared(startPoint, intersections[i]);
if (d < closestIntersectionDist)
{
leastPenetrationContact.Position = intersections[i];
closestIntersectionDist = d;
}
}
foreach (Vector2 axis in axes)
{
Range projectionA = axis.Projection(startPoint, endPoint),
projectionB = polygon.Projection(axis);
if (!projectionA.Overlaps(projectionB, out float penetrationDepth))
{
contact = null;
return(false);
}
if (penetrationDepth < leastPenetrationContact.PenetrationDepth)
{
leastPenetrationContact.PenetrationDepth = penetrationDepth;
leastPenetrationContact.Normal = projectionA.Min > projectionB.Min ? -axis : axis;
}
}
contact = leastPenetrationContact;
return(true);
}
public static bool TestIShapeWithConvexPolygon(IShape shape, Vector2 shapePos, Polygon polygon, ICollection <Vector2> axes, out Contact?contact)
{
Debug.Assert(!(shape is PolygonShape), "TestIShapeWithConvexPolygon can't handle PolygonShape correctly. Please use another polygon specific variant.");
Contact leastPenetrationContact = new Contact {
Position = shapePos,
PenetrationDepth = float.PositiveInfinity
};
foreach (Vector2 axis in axes)
{
Range projectionA = shape.Projection(shapePos, axis),
projectionB = polygon.Projection(axis);
if (!projectionA.Overlaps(projectionB, out float penetrationDepth))
{
contact = null;
return(false);
}
if (penetrationDepth < leastPenetrationContact.PenetrationDepth)
{
leastPenetrationContact.PenetrationDepth = penetrationDepth;
leastPenetrationContact.Normal = projectionA.Min > projectionB.Min ? -axis : axis;
}
}
// contact points
(Vector2 MaxProjVertex, Line Edge)edgeA = shape.FindBestClippingEdge(shapePos, leastPenetrationContact.Normal);
(Vector2 MaxProjVertex, Line Edge)edgeB = FindBestEdge(polygon, -leastPenetrationContact.Normal);
if (edgeA.Edge.PointA != edgeA.Edge.PointB)
{
Vector2[] contactPoints = CalculateContactPoints(edgeA, edgeB, leastPenetrationContact.Normal);
if (contactPoints.Length > 0)
{
leastPenetrationContact.Position = contactPoints[0];
}
}
contact = leastPenetrationContact;
return(true);
}
private static bool TestConvexPolygons(Polygon A, Polygon B, IEnumerable <Vector2> axes, out Contact?contact)
{
Contact leastPenetrationContact = new Contact {
Position = A[0],
PenetrationDepth = float.PositiveInfinity
};
foreach (Vector2 axis in axes)
{
Range projectionA = A.Projection(axis),
projectionB = B.Projection(axis);
if (!projectionA.Overlaps(projectionB, out float penetrationDepth))
{
contact = null;
return(false);
}
if (penetrationDepth < leastPenetrationContact.PenetrationDepth)
{
leastPenetrationContact.PenetrationDepth = penetrationDepth;
leastPenetrationContact.Normal = projectionA.Min > projectionB.Min ? -axis : axis;
}
}
// contact points
(Vector2 MaxProjVertex, Line Edge)edgeA = FindBestEdge(A, leastPenetrationContact.Normal);
(Vector2 MaxProjVertex, Line Edge)edgeB = FindBestEdge(B, -leastPenetrationContact.Normal);
Vector2[] contactPoints = CalculateContactPoints(edgeA, edgeB, leastPenetrationContact.Normal);
if (contactPoints.Length > 0)
{
leastPenetrationContact.Position = contactPoints[0];
}
contact = leastPenetrationContact;
return(true);
}
