/// <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) { if (rot == Rotation2.Zero) { return(ProjectAlongAxis(axis, pos, 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> /// 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> /// Calculates the distance that the given point is from this line. /// Will be nearly 0 if the point is on the line. /// </summary> /// <param name="line">The line</param> /// <param name="pos">The shift for the line</param> /// <param name="pt">The point that you want the distance from the line</param> /// <returns>The distance the point is from the line</returns> public static float Distance(Line2 line, Vector2 pos, Vector2 pt) { // As is typical for this type of question, we will solve along // the line treated as a linear space (which requires shifting // so the line goes through the origin). We will use that to find // the nearest point on the line to the given pt, then just // calculate the distance normally. Vector2 relPt = pt - line.Start - pos; float axisPart = Math2.Dot(relPt, line.Axis); float nearestAxisPart; if (axisPart < 0) { nearestAxisPart = 0; } else if (axisPart > line.Magnitude) { nearestAxisPart = line.Magnitude; } else { nearestAxisPart = axisPart; } Vector2 nearestOnLine = line.Start + pos + nearestAxisPart * line.Axis; return((pt - nearestOnLine).Length()); }
/// <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> /// 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--; } }