/// <summary>
/// Determines if the specified polygon at the specified position and rotation contains the specified point
/// </summary>
/// <param name="poly">The polygon</param>
/// <param name="pos">Origin of the polygon</param>
/// <param name="rot">Rotation of the polygon</param>
/// <param name="point">Point to check</param>
/// <param name="strict">True if the edges do not count as inside</param>
/// <returns>If the polygon at pos with rotation rot about its center contains point</returns>
public static bool Contains(Polygon2 poly, Vector2 pos, Rotation2 rot, Vector2 point, bool strict)
{
if (!Rect2.Contains(poly.AABB, pos, point, strict))
{
return(false);
}
// Calculate the area of the triangles constructed by the lines of the polygon. If it
// matches the area of the polygon, we're inside the polygon.
float myArea = 0;
var center = poly.Center + pos;
var last = Math2.Rotate(poly.Vertices[poly.Vertices.Length - 1], poly.Center, rot) + pos;
for (int i = 0; i < poly.Vertices.Length; i++)
{
var curr = Math2.Rotate(poly.Vertices[i], poly.Center, rot) + pos;
myArea += Math2.AreaOfTriangle(center, last, curr);
last = curr;
}
return(Math2.Approximately(myArea, poly.Area, poly.Area / 1000));
}
/// <summary>
/// Creates the ray trace polygons from the given polygon moving from start to end. The returned set of polygons
/// may not be the smallest possible set of polygons which perform this job.
///
/// In order to determine if polygon A intersects polygon B during a move from position S to E, you can check if
/// B intersects any of the polygons in CreateRaytraceAblesFromPolygon(A, E - S) when they are placed at S.
/// </summary>
/// <example>
/// <code>
/// Polygon2 a = ShapeUtils.CreateCircle(10, 0, 0, 5);
/// Polygon2 b = ShapeUtils.CreateCircle(15, 0, 0, 7);
///
/// Vector2 from = new Vector2(3, 3);
/// Vector2 to = new Vector2(15, 3);
/// Vector2 bloc = new Vector2(6, 3);
///
/// List<Polygon2> traces = Polygon2.CreateRaytraceAbles(a, to - from);
/// foreach (var trace in traces)
/// {
/// if (Polygon2.Intersects(trace, b, from, bloc, true))
/// {
/// Console.WriteLine("Intersects!");
/// break;
/// }
/// }
/// </code>
/// </example>
/// <param name="poly">The polygon that you want to move</param>
/// <param name="offset">The direction and magnitude that the polygon moves</param>
/// <returns>A set of polygons which completely contain the area that the polygon will intersect during a move
/// from the origin to offset.</returns>
public static List <Polygon2> CreateRaytraceAbles(Polygon2 poly, Vector2 offset)
{
var ourLinesAsRects = new List <Polygon2>();
if (Math2.Approximately(offset, Vector2.Zero))
{
ourLinesAsRects.Add(poly);
return(ourLinesAsRects);
}
for (int lineIndex = 0, nLines = poly.Lines.Length; lineIndex < nLines; lineIndex++)
{
var line = poly.Lines[lineIndex];
if (!Math2.IsOnLine(line.Start, line.End, line.Start + offset))
{
ourLinesAsRects.Add(new Polygon2(new Vector2[]
{
line.Start,
line.End,
line.End + offset,
line.Start + offset
}));
}
}
return(ourLinesAsRects);
}
/// <summary>
/// Checks the type of intersection between the two coincident lines.
/// </summary>
/// <param name="a">The first line</param>
/// <param name="b">The second line</param>
/// <param name="pos1">The offset for the first line</param>
/// <param name="pos2">The offset for the second line</param>
/// <returns>The type of intersection</returns>
public static unsafe LineInterType CheckCoincidentIntersectionType(Line2 a, Line2 b, Vector2 pos1, Vector2 pos2)
{
Vector2 relOrigin = a.Start + pos1;
float *projs = stackalloc float[4] {
0,
a.Magnitude,
Math2.Dot((b.Start + pos2) - relOrigin, a.Axis),
Math2.Dot((b.End + pos2) - relOrigin, a.Axis)
};
bool *isFromLine1 = stackalloc bool[4] {
true,
true,
false,
false
};
FindSortedOverlap(projs, isFromLine1);
if (Math2.Approximately(projs[1], projs[2]))
{
return(LineInterType.CoincidentPoint);
}
if (isFromLine1[0] == isFromLine1[1])
{
return(LineInterType.CoincidentNone);
}
return(LineInterType.CoincidentLine);
}
/// <summary>
/// Creates a bounding box with the specified upper-left and bottom-right.
/// Will autocorrect if min.X > max.X or min.Y > max.Y
/// </summary>
/// <param name="min">Min x, min y</param>
/// <param name="max">Max x, max y</param>
/// <exception cref="ArgumentException">If min and max do not make a box</exception>
public Rect2(Vector2 min, Vector2 max)
{
if (Math2.Approximately(min, max))
{
throw new ArgumentException($"Min is approximately max: min={min}, max={max} - tha'ts a point, not a box");
}
if (Math.Abs(min.X - max.X) <= Math2.DEFAULT_EPSILON)
{
throw new ArgumentException($"Min x is approximately max x: min={min}, max={max} - that's a line, not a box");
}
if (Math.Abs(min.Y - max.Y) <= Math2.DEFAULT_EPSILON)
{
throw new ArgumentException($"Min y is approximately max y: min={min}, max={max} - that's a line, not a box");
}
float tmpX1 = min.X, tmpX2 = max.X;
float tmpY1 = min.Y, tmpY2 = max.Y;
min.X = Math.Min(tmpX1, tmpX2);
min.Y = Math.Min(tmpY1, tmpY2);
max.X = Math.Max(tmpX1, tmpX2);
max.Y = Math.Max(tmpY1, tmpY2);
Min = min;
Max = max;
UpperRight = new Vector2(Max.X, Min.Y);
LowerLeft = new Vector2(Min.X, Max.Y);
Center = new Vector2((Min.X + Max.X) / 2, (Min.Y + Max.Y) / 2);
Width = Max.X - Min.X;
Height = Max.Y - Min.Y;
}
/// <summary>
/// Determines if the two lines are parallel. Shifting lines will not
/// effect the result.
/// </summary>
/// <param name="line1">The first line</param>
/// <param name="line2">The second line</param>
/// <returns>True if the lines are parallel, false otherwise</returns>
public static bool Parallel(Line2 line1, Line2 line2)
{
return(
Math2.Approximately(line1.Axis, line2.Axis) ||
Math2.Approximately(line1.Axis, -line2.Axis)
);
}
/// <summary>
/// Finds the line of overlap between the the two lines if there is
/// one. If the two lines are not coincident (i.e., if the infinite
/// lines are not the same) then they don't share a line of points.
/// If they are coincident, they may still share no points (two
/// seperate but coincident line segments), one point (they share
/// an edge), or infinitely many points (the share a coincident
/// line segment). In all but the last case, this returns false
/// and overlap is set to null. In the last case this returns true
/// and overlap is set to the line of overlap.
/// </summary>
/// <param name="a">The first line</param>
/// <param name="b">The second line</param>
/// <param name="pos1">The position of the first line</param>
/// <param name="pos2">the position of the second line</param>
/// <param name="overlap">Set to null or the line of overlap</param>
/// <returns>True if a and b overlap at infinitely many points,
/// false otherwise</returns>
public static unsafe bool LineOverlap(Line2 a, Line2 b, Vector2 pos1, Vector2 pos2, out Line2 overlap)
{
if (!Parallel(a, b))
{
overlap = null;
return(false);
}
if (!AlongInfiniteLine(a, pos1, b.Start + pos2))
{
overlap = null;
return(false);
}
Vector2 relOrigin = a.Start + pos1;
float *projs = stackalloc float[4] {
0,
a.Magnitude,
Math2.Dot((b.Start + pos2) - relOrigin, a.Axis),
Math2.Dot((b.End + pos2) - relOrigin, a.Axis)
};
bool *isFromLine1 = stackalloc bool[4] {
true,
true,
false,
false
};
FindSortedOverlap(projs, isFromLine1);
if (isFromLine1[0] == isFromLine1[1])
{
// at best we overlap at one point, most likely no overlap
overlap = null;
return(false);
}
if (Math2.Approximately(projs[1], projs[2]))
{
// Overlap at one point
overlap = null;
return(false);
}
overlap = new Line2(
relOrigin + projs[1] * a.Axis,
relOrigin + projs[2] * a.Axis
);
return(true);
}
/// <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>
/// 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>
/// 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>
/// 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));
}
