/// <summary>
/// Determines if the two polygons intersect using the Separating Axis Theorem.
/// The performance of this function depends on the number of unique normals
/// between the two polygons.
/// </summary>
/// <param name="poly1">First polygon</param>
/// <param name="poly2">Second polygon</param>
/// <param name="pos1">Offset for the vertices of the first polygon</param>
/// <param name="pos2">Offset for the vertices of the second polygon</param>
/// <param name="rot1">Rotation of the first polygon</param>
/// <param name="rot2">Rotation of the second polygon</param>
/// <param name="strict">
/// True if the two polygons must overlap a non-zero area for intersection,
/// false if they must overlap on at least one point for intersection.
/// </param>
/// <returns>True if the polygons overlap, false if they do not</returns>
public static bool IntersectsSat(Polygon2 poly1, Polygon2 poly2, Vector2 pos1, Vector2 pos2, Rotation2 rot1, Rotation2 rot2, bool strict)
{
if (rot1 == Rotation2.Zero && rot2 == Rotation2.Zero)
{
// This was a serious performance bottleneck so we speed up the fast case
HashSet <Vector2> seen = new HashSet <Vector2>();
Vector2[] poly1Verts = poly1.Vertices;
Vector2[] poly2Verts = poly2.Vertices;
for (int i = 0, len = poly1.Normals.Count; i < len; i++)
{
var axis = poly1.Normals[i];
var proj1 = ProjectAlongAxis(axis, pos1, poly1Verts);
var proj2 = ProjectAlongAxis(axis, pos2, poly2Verts);
if (!AxisAlignedLine2.Intersects(proj1, proj2, strict))
{
return(false);
}
seen.Add(axis);
}
for (int i = 0, len = poly2.Normals.Count; i < len; i++)
{
var axis = poly2.Normals[i];
if (seen.Contains(axis))
{
continue;
}
var proj1 = ProjectAlongAxis(axis, pos1, poly1Verts);
var proj2 = ProjectAlongAxis(axis, pos2, poly2Verts);
if (!AxisAlignedLine2.Intersects(proj1, proj2, strict))
{
return(false);
}
}
return(true);
}
foreach (var norm in poly1.Normals.Select((v) => Tuple.Create(v, rot1)).Union(poly2.Normals.Select((v) => Tuple.Create(v, rot2))))
{
var axis = Math2.Rotate(norm.Item1, Vector2.Zero, norm.Item2);
if (!IntersectsAlongAxis(poly1, poly2, pos1, pos2, rot1, rot2, strict, axis))
{
return(false);
}
}
return(true);
}
/// <summary>
/// Determines the actual location of the vertices of the given polygon
/// when at the given offset and rotation.
/// </summary>
/// <param name="polygon">The polygon</param>
/// <param name="offset">The polygons offset</param>
/// <param name="rotation">The polygons rotation</param>
/// <returns>The actualized polygon</returns>
public static Vector2[] ActualizePolygon(Polygon2 polygon, Vector2 offset, Rotation2 rotation)
{
int len = polygon.Vertices.Length;
Vector2[] result = new Vector2[len];
if (rotation != Rotation2.Zero)
{
for (int i = 0; i < len; i++)
{
result[i] = Math2.Rotate(polygon.Vertices[i], polygon.Center, rotation) + offset;
}
}
else
{
// performance sensitive section
int i = 0;
for (; i + 3 < len; i += 4)
{
result[i] = new Vector2(
polygon.Vertices[i].X + offset.X,
polygon.Vertices[i].Y + offset.Y
);
result[i + 1] = new Vector2(
polygon.Vertices[i + 1].X + offset.X,
polygon.Vertices[i + 1].Y + offset.Y
);
result[i + 2] = new Vector2(
polygon.Vertices[i + 2].X + offset.X,
polygon.Vertices[i + 2].Y + offset.Y
);
result[i + 3] = new Vector2(
polygon.Vertices[i + 3].X + offset.X,
polygon.Vertices[i + 3].Y + offset.Y
);
}
for (; i < len; i++)
{
result[i] = new Vector2(
polygon.Vertices[i].X + offset.X,
polygon.Vertices[i].Y + offset.Y
);
}
}
return(result);
}
/// <summary>
/// Returns a polygon that is created by rotated the original polygon
/// about its center by the specified amount. Returns the original polygon if
/// rot.Theta == 0.
/// </summary>
/// <returns>The rotated polygon.</returns>
/// <param name="original">Original.</param>
/// <param name="rot">Rot.</param>
public static Polygon2 GetRotated(Polygon2 original, Rotation2 rot)
{
if (rot.Theta == 0)
{
return(original);
}
var rotatedVerts = new Vector2[original.Vertices.Length];
for (var i = 0; i < original.Vertices.Length; i++)
{
rotatedVerts[i] = Math2.Rotate(original.Vertices[i], original.Center, rot);
}
return(new Polygon2(rotatedVerts));
}
/// <summary>
/// Determines if the given line contains the given point.
/// </summary>
/// <param name="line">The line to check</param>
/// <param name="pos">The offset for the line</param>
/// <param name="pt">The point to check</param>
/// <returns>True if pt is on the line, false otherwise</returns>
public static bool Contains(Line2 line, Vector2 pos, Vector2 pt)
{
// The horizontal/vertical checks are not required but are
// very fast to calculate and short-circuit the common case
// (false) very quickly
if (line.Horizontal)
{
return(Math2.Approximately(line.Start.Y + pos.Y, pt.Y) &&
AxisAlignedLine2.Contains(line.MinX, line.MaxX, pt.X - pos.X, false, false));
}
if (line.Vertical)
{
return(Math2.Approximately(line.Start.X + pos.X, pt.X) &&
AxisAlignedLine2.Contains(line.MinY, line.MaxY, pt.Y - pos.Y, false, false));
}
// Our line is not necessarily a linear space, but if we shift
// our line to the origin and adjust the point correspondingly
// then we have a linear space and the problem remains the same.
// Our line at the origin is just the infinite line with slope
// Axis. We can form an orthonormal basis of R2 as (Axis, Normal).
// Hence we can write pt = line_part * Axis + normal_part * Normal.
// where line_part and normal_part are floats. If the normal_part
// is 0, then pt = line_part * Axis, hence the point is on the
// infinite line.
// Since we are working with an orthonormal basis, we can find
// components with dot products.
// To check the finite line, we consider the start of the line
// the origin. Then the end of the line is line.Magnitude * line.Axis.
Vector2 lineStart = pos + line.Start;
float normalPart = Math2.Dot(pt - lineStart, line.Normal);
if (!Math2.Approximately(normalPart, 0))
{
return(false);
}
float axisPart = Math2.Dot(pt - lineStart, line.Axis);
return(axisPart > -Math2.DEFAULT_EPSILON &&
axisPart < line.Magnitude + Math2.DEFAULT_EPSILON);
}
/// <summary>
/// Creates a line from start to end
/// </summary>
/// <param name="start">Start</param>
/// <param name="end">End</param>
public Line2(Vector2 start, Vector2 end)
{
if (Math2.Approximately(start, end))
{
throw new ArgumentException($"start is approximately end - that's a point, not a line. start={start}, end={end}");
}
Start = start;
End = end;
Delta = End - Start;
Axis = Vector2.Normalize(Delta);
Normal = Vector2.Normalize(Math2.Perpendicular(Delta));
MagnitudeSquared = Delta.LengthSquared();
Magnitude = (float)Math.Sqrt(MagnitudeSquared);
MinX = Math.Min(Start.X, End.X);
MinY = Math.Min(Start.Y, End.Y);
MaxX = Math.Max(Start.X, End.X);
MaxY = Math.Max(Start.X, End.X);
Horizontal = Math.Abs(End.Y - Start.Y) <= Math2.DEFAULT_EPSILON;
Vertical = Math.Abs(End.X - Start.X) <= Math2.DEFAULT_EPSILON;
if (Vertical)
{
Slope = float.PositiveInfinity;
}
else
{
Slope = (End.Y - Start.Y) / (End.X - Start.X);
}
if (Vertical)
{
YIntercept = float.NaN;
}
else
{
// y = mx + b
// Start.Y = Slope * Start.X + b
// b = Start.Y - Slope * Start.X
YIntercept = Start.Y - Slope * Start.X;
}
}
/// <summary>
/// Determines the mtv to move pos1 by to prevent poly1 at pos1 from intersecting poly2 at pos2.
/// Returns null if poly1 and poly2 do not intersect.
/// </summary>
/// <param name="poly1">First polygon</param>
/// <param name="poly2">Second polygon</param>
/// <param name="pos1">Position of the first polygon</param>
/// <param name="pos2">Position of the second polygon</param>
/// <param name="rot1">Rotation of the first polyogn</param>
/// <param name="rot2">Rotation of the second polygon</param>
/// <returns>MTV to move poly1 to prevent intersection with poly2</returns>
public static Tuple <Vector2, float> IntersectMtv(Polygon2 poly1, Polygon2 poly2, Vector2 pos1, Vector2 pos2, Rotation2 rot1, Rotation2 rot2)
{
Vector2 bestAxis = Vector2.Zero;
float bestMagn = float.MaxValue;
foreach (var norm in poly1.Normals.Select((v) => Tuple.Create(v, rot1)).Union(poly2.Normals.Select((v) => Tuple.Create(v, rot2))))
{
var axis = Math2.Rotate(norm.Item1, Vector2.Zero, norm.Item2);
var mtv = IntersectMtvAlongAxis(poly1, poly2, pos1, pos2, rot1, rot2, axis);
if (!mtv.HasValue)
{
return(null);
}
else if (Math.Abs(mtv.Value) < Math.Abs(bestMagn))
{
bestAxis = axis;
bestMagn = mtv.Value;
}
}
return(Tuple.Create(bestAxis, bestMagn));
}
/// <summary>
/// Calculates the shortest distance and direction to go from poly1 at pos1 to poly2 at pos2. Returns null
/// if the polygons intersect.
/// </summary>
/// <returns>The distance.</returns>
/// <param name="poly1">First polygon</param>
/// <param name="poly2">Second polygon</param>
/// <param name="pos1">Origin of first polygon</param>
/// <param name="pos2">Origin of second polygon</param>
/// <param name="rot1">Rotation of first polygon</param>
/// <param name="rot2">Rotation of second polygon</param>
public static Tuple <Vector2, float> MinDistance(Polygon2 poly1, Polygon2 poly2, Vector2 pos1, Vector2 pos2, Rotation2 rot1, Rotation2 rot2)
{
if (rot1.Theta != 0 || rot2.Theta != 0)
{
throw new NotSupportedException("Finding the minimum distance between polygons requires calculating the rotated polygons. This operation is expensive and should be cached. " +
"Create the rotated polygons with Polygon2#GetRotated and call this function with Rotation2.Zero for both rotations.");
}
var axises = poly1.Normals.Union(poly2.Normals).Union(GetExtraMinDistanceVecsPolyPoly(poly1, poly2, pos1, pos2));
Vector2?bestAxis = null; // note this is the one with the longest distance
float bestDist = 0;
foreach (var norm in axises)
{
var proj1 = ProjectAlongAxis(poly1, pos1, rot1, norm);
var proj2 = ProjectAlongAxis(poly2, pos2, rot2, norm);
var dist = AxisAlignedLine2.MinDistance(proj1, proj2);
if (dist.HasValue && (bestAxis == null || dist.Value > bestDist))
{
bestDist = dist.Value;
if (proj2.Min < proj1.Min && dist > 0)
{
bestAxis = -norm;
}
else
{
bestAxis = norm;
}
}
}
if (!bestAxis.HasValue || Math2.Approximately(bestDist, 0))
{
return(null); // they intersect
}
return(Tuple.Create(bestAxis.Value, bestDist));
}
/// <summary>
/// Projects the polygon from the given points with origin pos along the specified axis.
/// </summary>
/// <param name="axis">Axis to project onto</param>
/// <param name="pos">Origin of polygon</param>
/// <param name="rot">Rotation of the polygon in radians</param>
/// <param name="center">Center of the polygon</param>
/// <param name="points">Points of polygon</param>
/// <returns>Projection of polygon of points at pos along axis</returns>
protected static AxisAlignedLine2 ProjectAlongAxis(Vector2 axis, Vector2 pos, Rotation2 rot, Vector2 center, params Vector2[] points)
{
float min = 0;
float max = 0;
for (int i = 0; i < points.Length; i++)
{
var polyPt = Math2.Rotate(points[i], center, rot);
var tmp = Math2.Dot(polyPt.X + pos.X, polyPt.Y + pos.Y, axis.X, axis.Y);
if (i == 0)
{
min = max = tmp;
}
else
{
min = Math.Min(min, tmp);
max = Math.Max(max, tmp);
}
}
return(new AxisAlignedLine2(axis, min, max));
}
/// <summary>
/// Calculates the shortest distance from the specified polygon to the specified point,
/// and the axis from polygon to pos.
///
/// Returns null if pt is contained in the polygon (not strictly).
/// </summary>
/// <returns>The distance form poly to pt.</returns>
/// <param name="poly">The polygon</param>
/// <param name="pos">Origin of the polygon</param>
/// <param name="rot">Rotation of the polygon</param>
/// <param name="pt">Point to check.</param>
public static Tuple <Vector2, float> MinDistance(Polygon2 poly, Vector2 pos, Rotation2 rot, Vector2 pt)
{
/*
* Definitions
*
* For each line in the polygon, find the normal of the line in the direction of outside the polygon.
* Call the side of the original line that contains none of the polygon "above the line". The other side is "below the line".
*
* If the point falls above the line:
* Imagine two additional lines that are normal to the line and fall on the start and end, respectively.
* For each of those two lines, call the side of the line that contains the original line "below the line". The other side is "above the line"
*
* If the point is above the line containing the start:
* The shortest vector is from the start to the point
*
* If the point is above the line containing the end:
* The shortest vector is from the end to the point
*
* Otherwise
* The shortest vector is from the line to the point
*
* If this is not true for ANY of the lines, the polygon does not contain the point.
*/
var last = Math2.Rotate(poly.Vertices[poly.Vertices.Length - 1], poly.Center, rot) + pos;
for (var i = 0; i < poly.Vertices.Length; i++)
{
var curr = Math2.Rotate(poly.Vertices[i], poly.Center, rot) + pos;
var axis = curr - last;
Vector2 norm;
if (poly.Clockwise)
{
norm = new Vector2(-axis.Y, axis.X);
}
else
{
norm = new Vector2(axis.Y, -axis.X);
}
norm = Vector2.Normalize(norm);
axis = Vector2.Normalize(axis);
var lineProjOnNorm = Vector2.Dot(norm, last);
var ptProjOnNorm = Vector2.Dot(norm, pt);
if (ptProjOnNorm > lineProjOnNorm)
{
var ptProjOnAxis = Vector2.Dot(axis, pt);
var stProjOnAxis = Vector2.Dot(axis, last);
if (ptProjOnAxis < stProjOnAxis)
{
var res = pt - last;
return(Tuple.Create(Vector2.Normalize(res), res.Length()));
}
var enProjOnAxis = Vector2.Dot(axis, curr);
if (ptProjOnAxis > enProjOnAxis)
{
var res = pt - curr;
return(Tuple.Create(Vector2.Normalize(res), res.Length()));
}
var distOnNorm = ptProjOnNorm - lineProjOnNorm;
return(Tuple.Create(norm, distOnNorm));
}
last = curr;
}
return(null);
}
/// <summary>
/// Determines if the two polygons intersect, inspired by the GJK algorithm. The
/// performance of this algorithm generally depends on how separated the
/// two polygons are.
///
/// This essentially acts as a directed search of the triangles in the
/// minkowski difference to check if any of them contain the origin.
///
/// The minkowski difference polygon has up to M*N possible vertices, where M is the
/// number of vertices in the first polygon and N is the number of vertices
/// in the second polygon.
/// </summary>
/// <param name="poly1">First polygon</param>
/// <param name="poly2">Second polygon</param>
/// <param name="pos1">Offset for the vertices of the first polygon</param>
/// <param name="pos2">Offset for the vertices of the second polygon</param>
/// <param name="rot1">Rotation of the first polygon</param>
/// <param name="rot2">Rotation of the second polygon</param>
/// <param name="strict">
/// True if the two polygons must overlap a non-zero area for intersection,
/// false if they must overlap on at least one point for intersection.
/// </param>
/// <returns>True if the polygons overlap, false if they do not</returns>
public static unsafe bool IntersectsGjk(Polygon2 poly1, Polygon2 poly2, Vector2 pos1, Vector2 pos2, Rotation2 rot1, Rotation2 rot2, bool strict)
{
Vector2[] verts1 = ActualizePolygon(poly1, pos1, rot1);
Vector2[] verts2 = ActualizePolygon(poly2, pos2, rot2);
Vector2 desiredAxis = new Vector2(
poly1.Center.X + pos1.X - poly2.Center.X - pos2.X,
poly2.Center.Y + pos1.Y - poly2.Center.Y - pos2.Y
);
if (Math2.Approximately(desiredAxis, Vector2.Zero))
{
desiredAxis = Vector2.UnitX;
}
else
{
desiredAxis.Normalize(); // cleanup rounding issues
}
var simplex = stackalloc Vector2[3];
int simplexIndex = -1;
bool simplexProper = true;
while (true)
{
if (simplexIndex < 2)
{
simplex[++simplexIndex] = CalculateSupport(verts1, verts2, desiredAxis);
float progressFromOriginTowardDesiredAxis = Math2.Dot(simplex[simplexIndex], desiredAxis);
if (progressFromOriginTowardDesiredAxis < -Math2.DEFAULT_EPSILON)
{
return(false); // no hope
}
if (progressFromOriginTowardDesiredAxis < Math2.DEFAULT_EPSILON)
{
if (Math2.Approximately(simplex[simplexIndex], Vector2.Zero))
{
// We've determined that the origin is a point on the
// edge of the minkowski difference. In fact, it's even
// a vertex. This means that the two polygons are just
// touching.
return(!strict);
}
// When we go to check the simplex, we can't assume that
// we know the origin will be in either AC or AB, as that
// assumption relies on this progress being strictly positive.
simplexProper = false;
}
if (simplexIndex == 0)
{
desiredAxis = -simplex[0];
desiredAxis.Normalize(); // resolve rounding issues
continue;
}
if (simplexIndex == 1)
{
// We only have 2 points; we need to select the third.
desiredAxis = Math2.TripleCross(simplex[1] - simplex[0], -simplex[1]);
if (Math2.Approximately(desiredAxis, Vector2.Zero))
{
// This means that the origin lies along the infinite
// line which goes through simplex[0] and simplex[1].
// We will choose a point perpendicular for now, but we
// will have to do extra work later to handle the fact that
// the origin won't be in regions AB or AC.
simplexProper = false;
desiredAxis = Math2.Perpendicular(simplex[1] - simplex[0]);
}
desiredAxis.Normalize(); // resolve rounding issues
continue;
}
}
Vector2 ac = simplex[0] - simplex[2];
Vector2 ab = simplex[1] - simplex[2];
Vector2 ao = -simplex[2];
Vector2 acPerp = Math2.TripleCross(ac, ab);
acPerp.Normalize(); // resolve rounding issues
float amountTowardsOriginAC = Math2.Dot(acPerp, ao);
if (amountTowardsOriginAC < -Math2.DEFAULT_EPSILON)
{
// We detected that the origin is in the AC region
desiredAxis = -acPerp;
simplexProper = true;
}
else
{
if (amountTowardsOriginAC < Math2.DEFAULT_EPSILON)
{
simplexProper = false;
}
// Could still be within the triangle.
Vector2 abPerp = Math2.TripleCross(ab, ac);
abPerp.Normalize(); // resolve rounding issues
float amountTowardsOriginAB = Math2.Dot(abPerp, ao);
if (amountTowardsOriginAB < -Math2.DEFAULT_EPSILON)
{
// We detected that the origin is in the AB region
simplex[0] = simplex[1];
desiredAxis = -abPerp;
simplexProper = true;
}
else
{
if (amountTowardsOriginAB < Math2.DEFAULT_EPSILON)
{
simplexProper = false;
}
if (simplexProper)
{
return(true);
}
// We've eliminated the standard cases for the simplex, i.e.,
// regions AB and AC. If the previous steps succeeded, this
// means we've definitively shown that the origin is within
// the triangle. However, if the simplex is improper, then
// we need to check the edges before we can be confident.
// We'll check edges first.
bool isOnABEdge = false;
if (Math2.IsBetweenLine(simplex[0], simplex[2], Vector2.Zero))
{
// we've determined the origin is on the edge AC.
// we'll swap B and C so that we're now on the edge
// AB, and handle like that case. abPerp and acPerp also swap,
// but we don't care about acPerp anymore
Vector2 tmp = simplex[0];
simplex[0] = simplex[1];
simplex[1] = tmp;
abPerp = acPerp;
isOnABEdge = true;
}
else if (Math2.IsBetweenLine(simplex[0], simplex[1], Vector2.Zero))
{
// we've determined the origin is on edge BC.
// we'll swap A and C so that we're now on the
// edge AB, and handle like that case. we'll need to
// recalculate abPerp
Vector2 tmp = simplex[2];
simplex[2] = simplex[0];
simplex[0] = tmp;
ab = simplex[1] - simplex[2];
ac = simplex[0] - simplex[2];
abPerp = Math2.TripleCross(ab, ac);
abPerp.Normalize();
isOnABEdge = true;
}
if (isOnABEdge || Math2.IsBetweenLine(simplex[1], simplex[2], Vector2.Zero))
{
// The origin is along the line AB. This means we'll either
// have another choice for A that wouldn't have done this,
// or the line AB is actually on the edge of the minkowski
// difference, and hence we are just touching.
// There is a case where this trick isn't going to work, in
// particular, if when you triangularize the polygon, the
// origin falls on an inner edge.
// In our case, at this point, we are going to have 4 points,
// which form a quadrilateral which contains the origin, but
// for which there is no way to draw a triangle out of the
// vertices that does not have the origin on the edge.
// I think though that the only way this happens would imply
// the origin is on simplex[1] <-> ogSimplex2 (we know this
// as that is what this if statement is for) and on
// simplex[0], (new) simplex[2], and I think it guarrantees
// we're in that case.
desiredAxis = -abPerp;
Vector2 ogSimplex2 = simplex[2];
simplex[2] = CalculateSupport(verts1, verts2, desiredAxis);
if (
Math2.Approximately(simplex[1], simplex[2]) ||
Math2.Approximately(ogSimplex2, simplex[2]) ||
Math2.Approximately(simplex[2], Vector2.Zero)
)
{
// we've shown that this is a true edge
return(!strict);
}
if (Math2.Dot(simplex[2], desiredAxis) <= 0)
{
// we didn't find a useful point!
return(!strict);
}
if (Math2.IsBetweenLine(simplex[0], simplex[2], Vector2.Zero))
{
// We've proven that we're contained in a quadrilateral
// Example of how we get here: C B A ogSimplex2
// (-1, -1), (-1, 0), (5, 5), (5, 0)
return(true);
}
if (Math2.IsBetweenLine(simplex[1], simplex[2], Vector2.Zero))
{
// We've shown that we on the edge
// Example of how we get here: C B A ogSimplex2
// (-32.66077,4.318787), (1.25, 0), (-25.41077, -0.006134033), (-32.66077, -0.006134033
return(!strict);
}
simplexProper = true;
continue;
}
// we can trust our results now as we know the point is
// not on an edge. we'll need to be confident in our
// progress check as well, so we'll skip the top of the
// loop
if (amountTowardsOriginAB < 0)
{
// in the AB region
simplex[0] = simplex[1];
desiredAxis = -abPerp;
}
else if (amountTowardsOriginAC < 0)
{
// in the AC region
desiredAxis = -acPerp;
}
else
{
// now we're sure the point is in the triangle
return(true);
}
simplex[1] = simplex[2];
simplex[2] = CalculateSupport(verts1, verts2, desiredAxis);
if (Math2.Dot(simplex[simplexIndex], desiredAxis) < 0)
{
return(false);
}
simplexProper = true;
continue;
}
}
simplex[1] = simplex[2];
simplexIndex--;
}
}
/// <summary>
/// Initializes a polygon with the specified vertices
/// </summary>
/// <param name="vertices">Vertices</param>
/// <exception cref="ArgumentNullException">If vertices is null</exception>
public Polygon2(Vector2[] vertices)
{
Vertices = vertices ?? throw new ArgumentNullException(nameof(vertices));
Normals = new List <Vector2>();
Vector2 tmp;
for (int i = 1; i < vertices.Length; i++)
{
tmp = Math2.MakeStandardNormal(Vector2.Normalize(Math2.Perpendicular(vertices[i] - vertices[i - 1])));
if (!Normals.Contains(tmp))
{
Normals.Add(tmp);
}
}
tmp = Math2.MakeStandardNormal(Vector2.Normalize(Math2.Perpendicular(vertices[0] - vertices[vertices.Length - 1])));
if (!Normals.Contains(tmp))
{
Normals.Add(tmp);
}
var min = new Vector2(vertices[0].X, vertices[0].Y);
var max = new Vector2(min.X, min.Y);
for (int i = 1; i < vertices.Length; i++)
{
min.X = Math.Min(min.X, vertices[i].X);
min.Y = Math.Min(min.Y, vertices[i].Y);
max.X = Math.Max(max.X, vertices[i].X);
max.Y = Math.Max(max.Y, vertices[i].Y);
}
AABB = new Rect2(min, max);
_LongestAxisLength = -1;
// Center, area, and lines
TrianglePartition = new Triangle2[Vertices.Length - 2];
float[] triangleSortKeys = new float[TrianglePartition.Length];
float area = 0;
Lines = new Line2[Vertices.Length];
Lines[0] = new Line2(Vertices[Vertices.Length - 1], Vertices[0]);
var last = Vertices[0];
Center = new Vector2(0, 0);
for (int i = 1; i < Vertices.Length - 1; i++)
{
var next = Vertices[i];
var next2 = Vertices[i + 1];
Lines[i] = new Line2(last, next);
var tri = new Triangle2(new Vector2[] { Vertices[0], next, next2 });
TrianglePartition[i - 1] = tri;
triangleSortKeys[i - 1] = -tri.Area;
area += tri.Area;
Center += tri.Center * tri.Area;
last = next;
}
Lines[Vertices.Length - 1] = new Line2(Vertices[Vertices.Length - 2], Vertices[Vertices.Length - 1]);
Array.Sort(triangleSortKeys, TrianglePartition);
Area = area;
Center /= area;
last = Vertices[Vertices.Length - 1];
var centToLast = (last - Center);
var angLast = Rotation2.Standardize((float)Math.Atan2(centToLast.Y, centToLast.X));
var cwCounter = 0;
var ccwCounter = 0;
var foundDefinitiveResult = false;
for (int i = 0; i < Vertices.Length; i++)
{
var curr = Vertices[i];
var centToCurr = (curr - Center);
var angCurr = Rotation2.Standardize((float)Math.Atan2(centToCurr.Y, centToCurr.X));
var clockwise = (angCurr < angLast && (angCurr - angLast) < Math.PI) || (angCurr - angLast) > Math.PI;
if (clockwise)
{
cwCounter++;
}
else
{
ccwCounter++;
}
Clockwise = clockwise;
if (Math.Abs(angLast - angCurr) > Math2.DEFAULT_EPSILON)
{
foundDefinitiveResult = true;
break;
}
last = curr;
centToLast = centToCurr;
angLast = angCurr;
}
if (!foundDefinitiveResult)
{
Clockwise = cwCounter > ccwCounter;
}
}
/// <summary>
/// Determines the vector, if any, to move poly at pos1 rotated rot1 to prevent intersection of rect
/// at pos2.
/// </summary>
/// <param name="poly">Polygon</param>
/// <param name="rect">Rectangle</param>
/// <param name="pos1">Origin of polygon</param>
/// <param name="pos2">Origin of rectangle</param>
/// <param name="rot1">Rotation of the polygon.</param>
/// <returns>The vector to move pos1 by or null</returns>
public static Tuple <Vector2, float> IntersectMTV(Polygon2 poly, Rect2 rect, Vector2 pos1, Vector2 pos2, Rotation2 rot1)
{
bool checkedX = false, checkedY = false;
Vector2 bestAxis = Vector2.Zero;
float bestMagn = float.MaxValue;
for (int i = 0; i < poly.Normals.Count; i++)
{
var norm = Math2.Rotate(poly.Normals[i], Vector2.Zero, rot1);
var mtv = IntersectMTVAlongAxis(poly, rect, pos1, pos2, rot1, norm);
if (!mtv.HasValue)
{
return(null);
}
if (Math.Abs(mtv.Value) < Math.Abs(bestMagn))
{
bestAxis = norm;
bestMagn = mtv.Value;
}
if (norm.X == 0)
{
checkedY = true;
}
if (norm.Y == 0)
{
checkedX = true;
}
}
if (!checkedX)
{
var mtv = IntersectMTVAlongAxis(poly, rect, pos1, pos2, rot1, Vector2.UnitX);
if (!mtv.HasValue)
{
return(null);
}
if (Math.Abs(mtv.Value) < Math.Abs(bestMagn))
{
bestAxis = Vector2.UnitX;
bestMagn = mtv.Value;
}
}
if (!checkedY)
{
var mtv = IntersectMTVAlongAxis(poly, rect, pos1, pos2, rot1, Vector2.UnitY);
if (!mtv.HasValue)
{
return(null);
}
if (Math.Abs(mtv.Value) < Math.Abs(bestMagn))
{
bestAxis = Vector2.UnitY;
bestMagn = mtv.Value;
}
}
return(Tuple.Create(bestAxis, bestMagn));
}
/// <summary>
/// Determines if the circle whose bounding boxs top left is at the first postion intersects the line
/// at the second position who is rotated the specified amount about the specified point.
/// </summary>
/// <param name="circle">The circle</param>
/// <param name="line">The line</param>
/// <param name="pos1">The top-left of the circles bounding box</param>
/// <param name="pos2">The origin of the line</param>
/// <param name="rot2">What rotation the line is under</param>
/// <param name="about2">What the line is rotated about</param>
/// <param name="strict">If overlap is required for intersection</param>
/// <returns>If the circle at pos1 intersects the line at pos2 rotated rot2 about about2</returns>
protected static bool CircleIntersectsLine(Circle2 circle, Line2 line, Vector2 pos1, Vector2 pos2, Rotation2 rot2, Vector2 about2, bool strict)
{
// Make more math friendly
var actualLine = new Line2(Math2.Rotate(line.Start, about2, rot2) + pos2, Math2.Rotate(line.End, about2, rot2) + pos2);
var circleCenter = new Vector2(pos1.X + circle.Radius, pos1.Y + circle.Radius);
// Check weird situations
if (actualLine.Horizontal)
{
return(CircleIntersectsHorizontalLine(circle, actualLine, circleCenter, strict));
}
if (actualLine.Vertical)
{
return(CircleIntersectsVerticalLine(circle, actualLine, circleCenter, strict));
}
// Goal:
// 1. Find closest distance, closestDistance, on the line to the circle (assuming the line was infinite)
// 1a Determine if closestPoint is intersects the circle according to strict
// - If it does not, we've shown there is no intersection.
// 2. Find closest point, closestPoint, on the line to the circle (assuming the line was infinite)
// 3. Determine if closestPoint is on the line (including edges)
// - If it is, we've shown there is intersection.
// 4. Determine which edge, edgeClosest, is closest to closestPoint
// 5. Determine if edgeClosest intersects the circle according to strict
// - If it does, we've shown there is intersection
// - If it does not, we've shown there is no intersection
// Step 1
// We're trying to find closestDistance
// Recall that the shortest line from a line to a point will be normal to the line
// Thus, the shortest distance from a line to a point can be found by projecting
// the line onto it's own normal vector and projecting the point onto the lines
// normal vector; the distance between those points is the shortest distance from
// the two points.
// The projection of a line onto its normal will be a single point, and will be same
// for any point on that line. So we pick a point that's convienent (the start or end).
var lineProjectedOntoItsNormal = Vector2.Dot(actualLine.Start, actualLine.Normal);
var centerOfCircleProjectedOntoNormalOfLine = Vector2.Dot(circleCenter, actualLine.Normal);
var closestDistance = Math.Abs(centerOfCircleProjectedOntoNormalOfLine - lineProjectedOntoItsNormal);
// Step 1a
if (strict)
{
if (closestDistance >= circle.Radius)
{
return(false);
}
}
else
{
if (closestDistance > circle.Radius)
{
return(false);
}
}
// Step 2
// We're trying to find closestPoint
// We can just walk the vector from the center to the closest point, which we know is on
// the normal axis and the distance closestDistance. However it's helpful to get the signed
// version End - Start to walk.
var signedDistanceCircleCenterToLine = lineProjectedOntoItsNormal - centerOfCircleProjectedOntoNormalOfLine;
var closestPoint = circleCenter - actualLine.Normal * signedDistanceCircleCenterToLine;
// Step 3
// Determine if closestPoint is on the line (including edges)
// We're going to accomplish this by projecting the line onto it's own axis and the closestPoint onto the lines
// axis. Then we have a 1D comparison.
var lineStartProjectedOntoLineAxis = Vector2.Dot(actualLine.Start, actualLine.Axis);
var lineEndProjectedOntoLineAxis = Vector2.Dot(actualLine.End, actualLine.Axis);
var closestPointProjectedOntoLineAxis = Vector2.Dot(closestPoint, actualLine.Axis);
if (AxisAlignedLine2.Contains(lineStartProjectedOntoLineAxis, lineEndProjectedOntoLineAxis, closestPointProjectedOntoLineAxis, false, true))
{
return(true);
}
// Step 4
// We're trying to find edgeClosest.
//
// We're going to reuse those projections from step 3.
//
// (for each "point" in the next paragraph I mean "point projected on the lines axis" but that's wordy)
//
// We know that the start is closest iff EITHER the start is less than the end and the
// closest point is less than the start, OR the start is greater than the end and
// closest point is greater than the end.
var closestEdge = Vector2.Zero;
if (lineStartProjectedOntoLineAxis < lineEndProjectedOntoLineAxis)
{
closestEdge = (closestPointProjectedOntoLineAxis <= lineStartProjectedOntoLineAxis) ? actualLine.Start : actualLine.End;
}
else
{
closestEdge = (closestPointProjectedOntoLineAxis >= lineEndProjectedOntoLineAxis) ? actualLine.Start : actualLine.End;
}
// Step 5
// Circle->Point intersection for closestEdge
var distToCircleFromClosestEdgeSq = (circleCenter - closestEdge).LengthSquared();
if (strict)
{
return(distToCircleFromClosestEdgeSq < (circle.Radius * circle.Radius));
}
else
{
return(distToCircleFromClosestEdgeSq <= (circle.Radius * circle.Radius));
}
// If you had trouble following, see the horizontal and vertical cases which are the same process but the projections
// are simpler
}
/// <summary>
/// Determines the minimum translation vector to be applied to the circle to
/// prevent overlap with the rectangle, when they are at their given positions.
/// </summary>
/// <param name="circle">The circle</param>
/// <param name="rect">The rectangle</param>
/// <param name="pos1">The top-left of the circles bounding box</param>
/// <param name="pos2">The rectangles origin</param>
/// <returns>MTV for circle at pos1 to prevent overlap with rect at pos2</returns>
public static Tuple <Vector2, float> IntersectMTV(Circle2 circle, Rect2 rect, Vector2 pos1, Vector2 pos2)
{
// Same as polygon rect, just converted to rects points
HashSet <Vector2> checkedAxis = new HashSet <Vector2>();
Vector2 bestAxis = Vector2.Zero;
float shortestOverlap = float.MaxValue;
Func <Vector2, bool> checkAxis = (axis) =>
{
var standard = Math2.MakeStandardNormal(axis);
if (!checkedAxis.Contains(standard))
{
checkedAxis.Add(standard);
var circleProj = Circle2.ProjectAlongAxis(circle, pos1, axis);
var rectProj = Rect2.ProjectAlongAxis(rect, pos2, axis);
var mtv = AxisAlignedLine2.IntersectMTV(circleProj, rectProj);
if (!mtv.HasValue)
{
return(false);
}
if (Math.Abs(mtv.Value) < Math.Abs(shortestOverlap))
{
bestAxis = axis;
shortestOverlap = mtv.Value;
}
}
return(true);
};
var circleCenter = new Vector2(pos1.X + circle.Radius, pos1.Y + circle.Radius);
int last = 4;
var lastVec = rect.UpperRight + pos2;
for (int curr = 0; curr < 4; curr++)
{
Vector2 currVec = Vector2.Zero;
switch (curr)
{
case 0:
currVec = rect.Min + pos2;
break;
case 1:
currVec = rect.LowerLeft + pos2;
break;
case 2:
currVec = rect.Max + pos2;
break;
case 3:
currVec = rect.UpperRight + pos2;
break;
}
// Test along circle center -> vector
if (!checkAxis(Vector2.Normalize(currVec - circleCenter)))
{
return(null);
}
// Test along line normal
if (!checkAxis(Vector2.Normalize(Math2.Perpendicular(currVec - lastVec))))
{
return(null);
}
last = curr;
lastVec = currVec;
}
return(Tuple.Create(bestAxis, shortestOverlap));
}
/// <summary>
/// Determines the minimum translation that must be applied the specified polygon (at the given position
/// and rotation) to prevent intersection with the circle (at its given rotation). If the two are not overlapping,
/// returns null.
///
/// Returns a tuple of the axis to move the polygon in (unit vector) and the distance to move the polygon.
/// </summary>
/// <param name="poly">The polygon</param>
/// <param name="circle">The circle</param>
/// <param name="pos1">The origin of the polygon</param>
/// <param name="pos2">The top-left of the circles bounding box</param>
/// <param name="rot1">The rotation of the polygon</param>
/// <returns></returns>
public static Tuple <Vector2, float> IntersectMTV(Polygon2 poly, Circle2 circle, Vector2 pos1, Vector2 pos2, Rotation2 rot1)
{
// We have two situations, either the circle is not strictly intersecting the polygon, or
// there exists at least one shortest line that you could push the polygon to prevent
// intersection with the circle.
// That line will either go from a vertix of the polygon to a point on the edge of the circle,
// or it will go from a point on a line of the polygon to the edge of the circle.
// If the line comes from a vertix of the polygon, the MTV will be along the line produced
// by going from the center of the circle to the vertix, and the distance can be found by
// projecting the cirle on that axis and the polygon on that axis and doing 1D overlap.
// If the line comes from a point on the edge of the polygon, the MTV will be along the
// normal of that line, and the distance can be found by projecting the circle on that axis
// and the polygon on that axis and doing 1D overlap.
// As with all SAT, if we find any axis that the circle and polygon do not overlap, we've
// proven they do not intersect.
// The worst case performance is related to 2x the number of vertices of the polygon, the same speed
// as for 2 polygons of equal number of vertices.
HashSet <Vector2> checkedAxis = new HashSet <Vector2>();
Vector2 bestAxis = Vector2.Zero;
float shortestOverlap = float.MaxValue;
Func <Vector2, bool> checkAxis = (axis) =>
{
var standard = Math2.MakeStandardNormal(axis);
if (!checkedAxis.Contains(standard))
{
checkedAxis.Add(standard);
var polyProj = Polygon2.ProjectAlongAxis(poly, pos1, rot1, axis);
var circleProj = Circle2.ProjectAlongAxis(circle, pos2, axis);
var mtv = AxisAlignedLine2.IntersectMTV(polyProj, circleProj);
if (!mtv.HasValue)
{
return(false);
}
if (Math.Abs(mtv.Value) < Math.Abs(shortestOverlap))
{
bestAxis = axis;
shortestOverlap = mtv.Value;
}
}
return(true);
};
var circleCenter = new Vector2(pos2.X + circle.Radius, pos2.Y + circle.Radius);
int last = poly.Vertices.Length - 1;
var lastVec = Math2.Rotate(poly.Vertices[last], poly.Center, rot1) + pos1;
for (int curr = 0; curr < poly.Vertices.Length; curr++)
{
var currVec = Math2.Rotate(poly.Vertices[curr], poly.Center, rot1) + pos1;
// Test along circle center -> vector
if (!checkAxis(Vector2.Normalize(currVec - circleCenter)))
{
return(null);
}
// Test along line normal
if (!checkAxis(Vector2.Normalize(Math2.Perpendicular(currVec - lastVec))))
{
return(null);
}
last = curr;
lastVec = currVec;
}
return(Tuple.Create(bestAxis, shortestOverlap));
}
/// <summary>
/// Initializes a polygon with the specified vertices
/// </summary>
/// <param name="vertices">Vertices</param>
/// <exception cref="ArgumentNullException">If vertices is null</exception>
public Polygon2(Vector2[] vertices)
{
if (vertices == null)
{
throw new ArgumentNullException(nameof(vertices));
}
Vertices = vertices;
Normals = new List <Vector2>();
Vector2 tmp;
for (int i = 1; i < vertices.Length; i++)
{
tmp = Math2.MakeStandardNormal(Vector2.Normalize(Math2.Perpendicular(vertices[i] - vertices[i - 1])));
if (!Normals.Contains(tmp))
{
Normals.Add(tmp);
}
}
tmp = Math2.MakeStandardNormal(Vector2.Normalize(Math2.Perpendicular(vertices[0] - vertices[vertices.Length - 1])));
if (!Normals.Contains(tmp))
{
Normals.Add(tmp);
}
var min = new Vector2(vertices[0].X, vertices[0].Y);
var max = new Vector2(min.X, min.Y);
for (int i = 1; i < vertices.Length; i++)
{
min.X = Math.Min(min.X, vertices[i].X);
min.Y = Math.Min(min.Y, vertices[i].Y);
max.X = Math.Max(max.X, vertices[i].X);
max.Y = Math.Max(max.Y, vertices[i].Y);
}
AABB = new Rect2(min, max);
Center = new Vector2(0, 0);
foreach (var vert in Vertices)
{
Center += vert;
}
Center *= (1.0f / Vertices.Length);
// Find longest axis
float longestAxisLenSq = -1;
for (int i = 1; i < vertices.Length; i++)
{
var vec = vertices[i] - vertices[i - 1];
longestAxisLenSq = Math.Max(longestAxisLenSq, vec.LengthSquared());
}
longestAxisLenSq = Math.Max(longestAxisLenSq, (vertices[0] - vertices[vertices.Length - 1]).LengthSquared());
LongestAxisLength = (float)Math.Sqrt(longestAxisLenSq);
// Area and lines
float area = 0;
Lines = new Line2[Vertices.Length];
var last = Vertices[Vertices.Length - 1];
for (int i = 0; i < Vertices.Length; i++)
{
var next = Vertices[i];
Lines[i] = new Line2(last, next);
area += Math2.AreaOfTriangle(last, next, Center);
last = next;
}
Area = area;
last = Vertices[Vertices.Length - 1];
var centToLast = (last - Center);
var angLast = Math.Atan2(centToLast.Y, centToLast.X);
var cwCounter = 0;
var ccwCounter = 0;
var foundDefinitiveResult = false;
for (int i = 0; i < Vertices.Length; i++)
{
var curr = Vertices[i];
var centToCurr = (curr - Center);
var angCurr = Math.Atan2(centToCurr.Y, centToCurr.X);
var clockwise = angCurr < angLast;
if (clockwise)
{
cwCounter++;
}
else
{
ccwCounter++;
}
Clockwise = clockwise;
if (Math.Abs(angLast - angCurr) > Math2.DEFAULT_EPSILON)
{
foundDefinitiveResult = true;
break;
}
last = curr;
centToLast = centToCurr;
angLast = angCurr;
}
if (!foundDefinitiveResult)
{
Clockwise = cwCounter > ccwCounter;
}
}
/// <summary>
/// Determines if the given point is along the infinite line described
/// by the given line shifted the given amount.
/// </summary>
/// <param name="line">The line</param>
/// <param name="pos">The shift for the line</param>
/// <param name="pt">The point</param>
/// <returns>True if pt is on the infinite line extension of the segment</returns>
public static bool AlongInfiniteLine(Line2 line, Vector2 pos, Vector2 pt)
{
float normalPart = Vector2.Dot(pt - pos - line.Start, line.Normal);
return(Math2.Approximately(normalPart, 0));
}
