Exemplo n.º 1
0
        /// <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);
        }
Exemplo n.º 2
0
        /// <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);
        }
Exemplo n.º 3
0
        /// <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));
        }
Exemplo n.º 4
0
        /// <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);
        }
Exemplo n.º 5
0
        /// <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;
            }
        }
Exemplo n.º 6
0
        /// <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));
        }
Exemplo n.º 7
0
        /// <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));
        }
Exemplo n.º 8
0
        /// <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));
        }
Exemplo n.º 9
0
        /// <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);
        }
Exemplo n.º 10
0
        /// <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--;
            }
        }
Exemplo n.º 11
0
        /// <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;
            }
        }
Exemplo n.º 12
0
        /// <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));
        }
Exemplo n.º 13
0
        /// <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
        }
Exemplo n.º 14
0
        /// <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));
        }
Exemplo n.º 15
0
        /// <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));
        }
Exemplo n.º 16
0
        /// <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;
            }
        }
Exemplo n.º 17
0
        /// <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));
        }