private bool CheckPolygonGrid(IShape A, Vector2 APos, IShape B, Vector2 BPos, out Contact[] contacts)
{
PolygonShape polygonShapeA = A as PolygonShape;
GridShape gridB = B as GridShape;
Rectangle polygonBoundingBox = polygonShapeA.BoundingBox + APos,
gridBoundingBox = gridB.BoundingBox + BPos;
// test grid bounds
if (!gridBoundingBox.Intersects(polygonBoundingBox))
{
contacts = null;
return(false);
}
Polygon polygonA = new Polygon(polygonShapeA.Shape);
polygonA.Translate(APos);
List <Contact> gridContacts = TestGrid(gridB, BPos, polygonBoundingBox,
(Polygon tilePolygon) => {
SAT.Test(polygonA, tilePolygon, out Contact? tileContact);
return(tileContact);
}
);
if (gridContacts.Count > 0)
{
contacts = gridContacts.ToArray();
return(true);
}
contacts = null;
return(false);
}
private static bool TestConcavePolygons(Polygon A, Polygon B, IEnumerable <Vector2> axes, out Contact?contact)
{
List <Vector2[]> componentsA = A.ConvexComponents(),
componentsB = B.ConvexComponents();
Vector2 polyAAnchor = A[0],
polyBAnchor = B[0];
foreach (Vector2[] componentA in componentsA)
{
Polygon componentPolyA = new Polygon(componentA);
componentPolyA.Translate(polyAAnchor);
foreach (Vector2[] componentB in componentsB)
{
Polygon componentPolyB = new Polygon(componentB);
componentPolyB.Translate(polyBAnchor);
if (TestConvexPolygons(componentPolyA, componentPolyB, axes, out contact))
{
return(true);
}
}
}
contact = null;
return(false);
}
public static bool TestIShapeWithConcavePolygon(IShape shape, Vector2 shapePos, Polygon concavePolygon, out Contact?contact)
{
List <Vector2[]> concavePolygonComponents = concavePolygon.ConvexComponents();
Vector2 concavePolygonAnchor = concavePolygon[0];
Vector2[] shapeAxes = shape.CalculateAxes();
Vector2[] axes;
foreach (Vector2[] component in concavePolygonComponents)
{
Polygon componentPolygon = new Polygon(component);
componentPolygon.Translate(concavePolygonAnchor);
axes = new Vector2[shapeAxes.Length + componentPolygon.Normals.Length];
shapeAxes.CopyTo(axes, 0);
componentPolygon.Normals.CopyTo(axes, shapeAxes.Length);
if (TestIShapeWithConvexPolygon(shape, shapePos, componentPolygon, axes, out contact))
{
return(true);
}
}
contact = null;
return(false);
}
public Range Projection(Vector2 shapePosition, Vector2 axis)
{
Polygon polygon = new Polygon(Shape);
polygon.Translate(shapePosition);
return(polygon.Projection(axis));
}
public (Vector2 MaxProjectionVertex, Line Edge) FindBestClippingEdge(Vector2 shapePosition, Vector2 normal)
{
Polygon polygon = new Polygon(Shape);
polygon.Translate(shapePosition);
return(SAT.FindBestEdge(polygon, normal));
}
private bool CheckBoxBox(IShape A, Vector2 APos, IShape B, Vector2 BPos, out Contact[] contacts)
{
BoxShape boxA = A as BoxShape,
shapeB = B as BoxShape;
// AABB x AABB
if (System.Math.Abs(boxA.Rotation) < Math.Epsilon && System.Math.Abs(shapeB.Rotation) < Math.Epsilon)
{
Vector2 translation = BPos - APos;
Vector2 overlap = (boxA.HalwidthExtents + shapeB.HalwidthExtents) - Math.Abs(translation);
if (overlap.X < 0f || overlap.Y < 0f)
{
contacts = null;
return(false);
}
contacts = new Contact[1];
if (overlap.X < overlap.Y)
{
float sign = Math.Sign(translation.X);
contacts[0] = new Contact {
Position = new Vector2(APos.X + boxA.HalwidthExtents.X * sign, BPos.Y),
Normal = new Vector2(sign, 0f),
PenetrationDepth = Math.Abs(overlap.X)
};
}
else
{
float sign = Math.Sign(translation.Y);
contacts[0] = new Contact {
Position = new Vector2(BPos.X, APos.Y + boxA.HalwidthExtents.Y * sign),
Normal = new Vector2(0f, sign),
PenetrationDepth = Math.Abs(overlap.Y)
};
}
return(true);
}
// OBB x OBB
Polygon polygonA = new Polygon(boxA.Shape), polygonB = new Polygon(shapeB.Shape);
polygonA.Translate(APos);
polygonB.Translate(BPos);
if (SAT.Test(polygonA, polygonB, out Contact? contact))
{
contacts = new Contact[] {
contact.Value
};
return(true);
}
contacts = null;
return(false);
}
/// <summary>
/// Tests a IShape with another IShape with explicit axes.
/// </summary>
/// <param name="shapeA">First IShape in the test.</param>
/// <param name="posA">First IShape position.</param>
/// <param name="shapeB">Second IShape in the test.</param>
/// <param name="posB">Second IShape position.</param>
/// <param name="axes">Separating axes to use when testing.</param>
/// <param name="contact">Contact info about the test, Null otherwise.</param>
/// <returns>True if they're intersecting, False otherwise.</returns>
public static bool Test(IShape shapeA, Vector2 posA, IShape shapeB, Vector2 posB, ICollection <Vector2> axes, out Contact?contact)
{
Polygon polygonA = null,
polygonB = null;
if (shapeA is PolygonShape polygonShapeA)
{
polygonA = new Polygon(polygonShapeA.Shape);
polygonA.Translate(posA);
}
if (shapeB is PolygonShape polygonShapeB)
{
polygonB = new Polygon(polygonShapeB.Shape);
polygonB.Translate(posB);
}
if (polygonA != null)
{
if (polygonB != null)
{
if (polygonA.IsConvex)
{
if (polygonB.IsConvex)
{
return(TestConvexPolygons(polygonA, polygonB, axes, out contact));
}
return(TestConvexPolygonWithConcavePolygon(polygonA, polygonB, axes, out contact));
}
if (polygonB.IsConvex)
{
return(TestConvexPolygonWithConcavePolygon(polygonB, polygonA, axes, out contact));
}
return(TestConcavePolygons(polygonA, polygonB, axes, out contact));
}
if (polygonA.IsConvex)
{
return(TestIShapeWithConvexPolygon(shapeB, posB, polygonA, axes, out contact));
}
return(TestIShapeWithConcavePolygon(shapeB, posB, polygonA, axes, out contact));
}
else if (polygonB != null)
{
if (polygonB.IsConvex)
{
return(TestIShapeWithConvexPolygon(shapeA, posA, polygonB, axes, out contact));
}
return(TestIShapeWithConcavePolygon(shapeA, posA, polygonB, axes, out contact));
}
return(TestIShapeWithIShape(shapeA, posA, shapeB, posB, axes, out contact));
}
public void DebugRender(Vector2 position, Color color)
{
#if DEBUG
// background grid
//Debug.DrawGrid(TileSize, Columns, Rows, position, BackgroundGridColor);
// collision tiles
foreach ((int Column, int Row, TileShape Shape)tile in Tiles())
{
if (tile.Shape == null)
{
continue;
}
Vector2 tilePos = position - Origin + new Vector2(tile.Column * TileSize.Width, tile.Row * TileSize.Height);
if (tile.Shape is BoxTileShape)
{
Debug.Draw.PhysicsBodiesLens.Rectangle.AtWorld(
new Rectangle(tilePos, TileSize), false, CollisionTilesColor
);
}
else if (tile.Shape is PolygonTileShape polygonTile)
{
#if !DEBUG_RENDER_FORCE_DRAWING_COMPONENTS
if (polygonTile.Polygon.IsConvex)
{
Polygon p = new Polygon(polygonTile.Polygon);
p.Translate(tilePos);
Debug.Draw.PhysicsBodiesLens.Polygon.AtWorld(p, false, CollisionTilesColor);
}
else
{
#endif
List <Vector2[]> components = polygonTile.Polygon.ConvexComponents();
Vector2 anchorVertex = polygonTile.Polygon[0];
foreach (Vector2[] component in components)
{
Debug.Draw.PhysicsBodiesLens.Lines.AtWorld(
component, true, tilePos + anchorVertex, CollisionTilesColor
);
}
if (DebugRenderDetailed)
{
Debug.Draw.String.AtWorld($"{components.Count}", tilePos + TileSize / 2f, Color.White);
}
#if !DEBUG_RENDER_FORCE_DRAWING_COMPONENTS
}
#endif
}
}
// bounding box
//Debug.DrawRectangle(new Rectangle(position, Debug.Transform(BoundingBox)), Color.Indigo);
#endif
}
public PolygonShape(IEnumerable <Vector2> points, bool centralize = true)
{
_normalizedPolygon = new Polygon(points);
if (centralize)
{
_normalizedPolygon.Translate(-_normalizedPolygon.Center);
}
Recalculate();
}
private static bool TestConcavePolygons(Polygon A, Polygon B, out Contact?contact)
{
List <Vector2[]> componentsA = A.ConvexComponents(),
componentsB = B.ConvexComponents();
Vector2 polyAAnchor = A[0],
polyBAnchor = B[0];
Vector2[] axes;
int i;
foreach (Vector2[] componentA in componentsA)
{
Polygon componentPolyA = new Polygon(componentA);
componentPolyA.Translate(polyAAnchor);
foreach (Vector2[] componentB in componentsB)
{
Polygon componentPolyB = new Polygon(componentB);
componentPolyB.Translate(polyBAnchor);
axes = new Vector2[componentPolyA.Normals.Length + componentPolyB.Normals.Length];
i = 0;
// component polygon A axes
foreach (Vector2 normal in componentPolyA.Normals)
{
axes[i] = normal;
i++;
}
// component polygon B axes
foreach (Vector2 normal in componentPolyB.Normals)
{
axes[i] = normal;
i++;
}
if (TestConvexPolygons(componentPolyA, componentPolyB, axes, out contact))
{
return(true);
}
}
}
contact = null;
return(false);
}
private List <Contact> TestGrid(GridShape grid, Vector2 gridPos, Rectangle otherBoundingBox, System.Func <Polygon, Contact?> SAT)
{
List <Contact> contacts = new List <Contact>();
Polygon boxTilePolygon = new Polygon(grid.BoxTilePolygon);
boxTilePolygon.Translate(gridPos);
(int column, int row)start = grid.ConvertPosition(gridPos, otherBoundingBox.TopLeft - Vector2.One),
end = grid.ConvertPosition(gridPos, otherBoundingBox.BottomRight + Vector2.One);
List <Polygon> tilePolygons = new List <Polygon>();
foreach ((int Column, int Row, GridShape.TileShape Shape)tile in grid.Tiles(start, end))
{
tilePolygons.Clear();
if (tile.Shape != null)
{
if (tile.Shape is GridShape.BoxTileShape boxShape)
{
tilePolygons.Add(boxShape.CreateCollisionPolygon(grid, gridPos, tile.Column, tile.Row));
}
else if (tile.Shape is GridShape.PolygonTileShape polygonShape)
{
tilePolygons.AddRange(polygonShape.CreateCollisionPolygons(grid, gridPos, tile.Column, tile.Row));
}
else
{
throw new System.InvalidOperationException($"Unable to find shape '{tile.Shape.GetType().Name}'.");
}
}
Location cell = new Location(tile.Column, tile.Row);
foreach (Polygon tilePolygon in tilePolygons)
{
Contact?contact = SAT(tilePolygon);
if (contact == null)
{
continue;
}
contacts.Add(new Contact(contact.Value.Position, contact.Value.Normal, contact.Value.PenetrationDepth, cell));
}
}
return(contacts);
}
///
/// <summary>
/// Tests a line segment with a IShape without explicit axes.
/// The axes will be infered by the IShape normals and line segment direction and perpendicular.
/// </summary>
/// <param name="startPoint">Start point at line segment.</param>
/// <param name="endPoint">End point at line segment.</param>
/// <param name="shape">IShape element to test.</param>
/// <param name="shapePos">IShape position.</param>
/// <param name="contact">Contact info about the test, Null otherwise.</param>
/// <returns>True if they're intersecting, False otherwise.</returns>
public static bool Test(Vector2 startPoint, Vector2 endPoint, IShape shape, Vector2 shapePos, out Contact?contact)
{
if (shape is PolygonShape polygonShape)
{
Polygon polygon = new Polygon(polygonShape.Shape);
polygon.Translate(shapePos);
if (polygon.IsConvex)
{
return(TestLineSegmentWithConvexPolygon(startPoint, endPoint, polygon, out contact));
}
return(TestLineSegmentWithConcavePolygon(startPoint, endPoint, polygon, out contact));
}
return(TestLineSegmentWithIShape(startPoint, endPoint, shape, shapePos, out contact));
}
public static bool TestLineSegmentWithConcavePolygon(Vector2 startPoint, Vector2 endPoint, Polygon concavePolygon, IEnumerable <Vector2> axes, out Contact?contact)
{
List <Vector2[]> concavePolygonComponents = concavePolygon.ConvexComponents();
Vector2 concavePolygonAnchor = concavePolygon[0];
foreach (Vector2[] component in concavePolygonComponents)
{
Polygon componentPolygon = new Polygon(component);
componentPolygon.Translate(concavePolygonAnchor);
if (TestLineSegmentWithConvexPolygon(startPoint, endPoint, componentPolygon, axes, out contact))
{
return(true);
}
}
contact = null;
return(false);
}
private static bool TestConvexPolygonWithConcavePolygon(Polygon convexPolygon, Polygon concavePolygon, IEnumerable <Vector2> axes, out Contact?contact)
{
List <Vector2[]> concavePolygonComponents = concavePolygon.ConvexComponents();
Vector2 concavePolygonAnchor = concavePolygon[0];
foreach (Vector2[] component in concavePolygonComponents)
{
Polygon componentPolygon = new Polygon(component);
componentPolygon.Translate(concavePolygonAnchor);
if (TestConvexPolygons(convexPolygon, componentPolygon, axes, out contact))
{
return(true);
}
}
contact = null;
return(false);
}
public static bool TestIShapeWithConcavePolygon(IShape shape, Vector2 shapePos, Polygon concavePolygon, ICollection <Vector2> axes, out Contact?contact)
{
List <Vector2[]> concavePolygonComponents = concavePolygon.ConvexComponents();
Vector2 concavePolygonAnchor = concavePolygon[0];
foreach (Vector2[] component in concavePolygonComponents)
{
Polygon componentPolygon = new Polygon(component);
componentPolygon.Translate(concavePolygonAnchor);
if (TestIShapeWithConvexPolygon(shape, shapePos, componentPolygon, axes, out contact))
{
return(true);
}
}
contact = null;
return(false);
}
private static bool TestConvexPolygonWithConcavePolygon(Polygon convexPolygon, Polygon concavePolygon, out Contact?contact)
{
List <Vector2[]> concavePolygonComponents = concavePolygon.ConvexComponents();
Vector2 concavePolygonAnchor = concavePolygon[0];
Vector2[] axes;
int i;
foreach (Vector2[] component in concavePolygonComponents)
{
Polygon componentPolygon = new Polygon(component);
componentPolygon.Translate(concavePolygonAnchor);
axes = new Vector2[componentPolygon.Normals.Length + convexPolygon.Normals.Length];
i = 0;
// component polygon A axes
foreach (Vector2 normal in convexPolygon.Normals)
{
axes[i] = normal;
i++;
}
// component polygon B axes
foreach (Vector2 normal in componentPolygon.Normals)
{
axes[i] = normal;
i++;
}
if (TestConvexPolygons(convexPolygon, componentPolygon, axes, out contact))
{
return(true);
}
}
contact = null;
return(false);
}
public static bool TestLineSegmentWithConcavePolygon(Vector2 startPoint, Vector2 endPoint, Polygon concavePolygon, out Contact?contact)
{
List <Vector2[]> concavePolygonComponents = concavePolygon.ConvexComponents();
Vector2 concavePolygonAnchor = concavePolygon[0];
Vector2 direction = (endPoint - startPoint).Normalized();
Vector2[] axes;
int i;
foreach (Vector2[] component in concavePolygonComponents)
{
Polygon componentPolygon = new Polygon(component);
componentPolygon.Translate(concavePolygonAnchor);
axes = new Vector2[2 + componentPolygon.Normals.Length];
axes[0] = direction;
axes[1] = direction.PerpendicularCW();
i = 2;
// component polygon A axes
foreach (Vector2 normal in componentPolygon.Normals)
{
axes[i] = normal;
i++;
}
if (TestLineSegmentWithConvexPolygon(startPoint, endPoint, componentPolygon, axes, out contact))
{
return(true);
}
}
contact = null;
return(false);
}
private bool CheckPolygonPolygon(IShape A, Vector2 APos, IShape B, Vector2 BPos, out Contact[] contacts)
{
PolygonShape shapeA = A as PolygonShape,
shapeB = B as PolygonShape;
Polygon polygonA = new Polygon(shapeA.Shape),
polygonB = new Polygon(shapeB.Shape);
polygonA.Translate(APos);
polygonB.Translate(BPos);
if (SAT.Test(polygonA, polygonB, out Contact? contact))
{
contacts = new Contact[] {
contact.Value
};
return(true);
}
contacts = null;
return(false);
}
private bool CheckCirclePolygon(IShape A, Vector2 APos, IShape B, Vector2 BPos, out Contact[] contacts)
{
CircleShape circleA = A as CircleShape;
PolygonShape polyB = B as PolygonShape;
Polygon polygonB = new Polygon(polyB.Shape);
polygonB.Translate(BPos);
Vector2 closestPoint = polygonB.ClosestPoint(APos);
Vector2 d = closestPoint - APos;
if (Vector2.Dot(d, d) > circleA.Radius * circleA.Radius)
{
contacts = null;
return(false);
}
if (SAT.Test(circleA, APos, polygonB, out Contact? contact))
{
if (contact == null)
{
contacts = new Contact[0];
}
else
{
contacts = new Contact[] {
new Contact(closestPoint, contact.Value.Normal, contact.Value.PenetrationDepth, null)
};
}
return(true);
}
contacts = null;
return(false);
}
public PolygonShape(params Vector2[] points)
{
_normalizedPolygon = new Polygon(points);
_normalizedPolygon.Translate(-_normalizedPolygon.Center);
Recalculate();
}