private static IEnumerable <Vector2> GetExtraMinDistanceVecsPolyPoly(Polygon2 poly1, Polygon2 poly2, Vector2 pos1, Vector2 pos2) { foreach (var vert in poly1.Vertices) { foreach (var vert2 in poly2.Vertices) { var roughAxis = ((vert2 + pos2) - (vert + pos1)); roughAxis.Normalize(); yield return(Math2.MakeStandardNormal(roughAxis)); } } }
/// <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 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> /// 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 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)); }