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);
}
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 CheckBoxCircle(IShape A, Vector2 APos, IShape B, Vector2 BPos, out Contact[] contacts)
{
BoxShape boxA = A as BoxShape;
CircleShape circleB = B as CircleShape;
Vector2 closestPoint = boxA.ClosestPoint(APos, BPos);
Vector2 diff = closestPoint - BPos;
if (Vector2.Dot(diff, diff) > circleB.Radius * circleB.Radius)
{
contacts = null;
return(false);
}
if (SAT.Test(boxA, APos, circleB, BPos, boxA.Axes, 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);
}
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);
}
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 CheckCircleGrid(IShape A, Vector2 APos, IShape B, Vector2 BPos, out Contact[] contacts)
{
CircleShape circleA = A as CircleShape;
GridShape gridB = B as GridShape;
// test grid bounds
Vector2 closestPoint = new Rectangle(BPos, gridB.Size).ClosestPoint(APos);
Vector2 diff = closestPoint - APos;
if (Vector2.Dot(diff, diff) > circleA.Radius * circleA.Radius)
{
contacts = null;
return(false);
}
List <Contact> gridContacts = TestGrid(gridB, BPos, new Rectangle(APos - circleA.Radius, APos + circleA.Radius),
(Polygon tilePolygon) => {
SAT.Test(circleA, APos, tilePolygon, out Contact? tileContact);
if (tileContact == null)
{
return(null);
}
return(new Contact(closestPoint, tileContact.Value.Normal, tileContact.Value.PenetrationDepth, null));
}
);
if (gridContacts.Count > 0)
{
contacts = gridContacts.ToArray();
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);
}