/// <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)); }