Windows Phone Labyrinth

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;
using FarseerPhysics.Collision;
using Microsoft.Xna.Framework;

namespace FarseerPhysics.Common
{
#if !(XBOX360)
    [DebuggerDisplay("Count = {Count} Vertices = {ToString()}")]
#endif
    public class Vertices : List<Vector2>
    {
        public Vertices()
        {
        }

        public Vertices(int capacity)
        {
            Capacity = capacity;
        }

        public Vertices(Vector2[] vector2)
        {
            for (int i = 0; i < vector2.Length; i++)
            {
                Add(vector2[i]);
            }
        }

        public Vertices(IList<Vector2> vertices)
        {
            for (int i = 0; i < vertices.Count; i++)
            {
                Add(vertices[i]);
            }
        }

        /// <summary>
        /// Nexts the index.
        /// </summary>
        /// <param name="index">The index.</param>
        /// <returns></returns>
        public int NextIndex(int index)
        {
            if (index == Count - 1)
            {
                return 0;
            }
            return index + 1;
        }

        public Vector2 NextVertex(int index)
        {
            return this[NextIndex(index)];
        }

        /// <summary>
        /// Gets the previous index.
        /// </summary>
        /// <param name="index">The index.</param>
        /// <returns></returns>
        public int PreviousIndex(int index)
        {
            if (index == 0)
            {
                return Count - 1;
            }
            return index - 1;
        }

        public Vector2 PreviousVertex(int index)
        {
            return this[PreviousIndex(index)];
        }

        /// <summary>
        /// Gets the signed area.
        /// </summary>
        /// <returns></returns>
        public float GetSignedArea()
        {
            int i;
            float area = 0;

            for (i = 0; i < Count; i++)
            {
                int j = (i + 1) % Count;
                area += this[i].X * this[j].Y;
                area -= this[i].Y * this[j].X;
            }
            area /= 2.0f;
            return area;
        }

        /// <summary>
        /// Gets the area.
        /// </summary>
        /// <returns></returns>
        public float GetArea()
        {
            int i;
            float area = 0;

            for (i = 0; i < Count; i++)
            {
                int j = (i + 1) % Count;
                area += this[i].X * this[j].Y;
                area -= this[i].Y * this[j].X;
            }
            area /= 2.0f;
            return (area < 0 ? -area : area);
        }

        /// <summary>
        /// Gets the centroid.
        /// </summary>
        /// <returns></returns>
        public Vector2 GetCentroid()
        {
            // Same algorithm is used by Box2D

            Vector2 c = Vector2.Zero;
            float area = 0.0f;

            const float inv3 = 1.0f / 3.0f;
            Vector2 pRef = Vector2.Zero;
            for (int i = 0; i < Count; ++i)
            {
                // Triangle vertices.
                Vector2 p1 = pRef;
                Vector2 p2 = this[i];
                Vector2 p3 = i + 1 < Count ? this[i + 1] : this[0];

                Vector2 e1 = p2 - p1;
                Vector2 e2 = p3 - p1;

                float D = MathUtils.Cross(e1, e2);

                float triangleArea = 0.5f * D;
                area += triangleArea;

                // Area weighted centroid
                c += triangleArea * inv3 * (p1 + p2 + p3);
            }

            // Centroid
            c *= 1.0f / area;
            return c;
        }

        /// <summary>
        /// Gets the radius based on area.
        /// </summary>
        /// <returns></returns>
        public float GetRadius()
        {
            float area = GetSignedArea();

            double radiusSqrd = (double)area / MathHelper.Pi;
            if (radiusSqrd < 0)
            {
                radiusSqrd *= -1;
            }

            return (float)Math.Sqrt(radiusSqrd);
        }

        /// <summary>
        /// Returns an AABB for vertex.
        /// </summary>
        /// <returns></returns>
        public AABB GetCollisionBox()
        {
            AABB aabb;
            Vector2 lowerBound = new Vector2(float.MaxValue, float.MaxValue);
            Vector2 upperBound = new Vector2(float.MinValue, float.MinValue);

            for (int i = 0; i < Count; ++i)
            {
                if (this[i].X < lowerBound.X)
                {
                    lowerBound.X = this[i].X;
                }
                if (this[i].X > upperBound.X)
                {
                    upperBound.X = this[i].X;
                }

                if (this[i].Y < lowerBound.Y)
                {
                    lowerBound.Y = this[i].Y;
                }
                if (this[i].Y > upperBound.Y)
                {
                    upperBound.Y = this[i].Y;
                }
            }

            aabb.LowerBound = lowerBound;
            aabb.UpperBound = upperBound;

            return aabb;
        }

        public void Translate(Vector2 vector)
        {
            Translate(ref vector);
        }

        /// <summary>
        /// Translates the vertices with the specified vector.
        /// </summary>
        /// <param name="vector">The vector.</param>
        public void Translate(ref Vector2 vector)
        {
            for (int i = 0; i < Count; i++)
                this[i] = Vector2.Add(this[i], vector);
        }

        /// <summary>
        /// Scales the vertices with the specified vector.
        /// </summary>
        /// <param name="value">The Value.</param>
        public void Scale(ref Vector2 value)
        {
            for (int i = 0; i < Count; i++)
                this[i] = Vector2.Multiply(this[i], value);
        }

        /// <summary>
        /// Rotate the vertices with the defined value in radians.
        /// </summary>
        /// <param name="value">The amount to rotate by in radians.</param>
        public void Rotate(float value)
        {
            Matrix rotationMatrix;
            Matrix.CreateRotationZ(value, out rotationMatrix);

            for (int i = 0; i < Count; i++)
                this[i] = Vector2.Transform(this[i], rotationMatrix);
        }

        /// <summary>
        /// Assuming the polygon is simple; determines whether the polygon is convex.
        /// NOTE: It will also return false if the input contains colinear edges.
        /// </summary>
        /// <returns>
        /// 	<c>true</c> if it is convex; otherwise, <c>false</c>.
        /// </returns>
        public bool IsConvex()
        {
            // Ensure the polygon is convex and the interior
            // is to the left of each edge.
            for (int i = 0; i < Count; ++i)
            {
                int i1 = i;
                int i2 = i + 1 < Count ? i + 1 : 0;
                Vector2 edge = this[i2] - this[i1];

                for (int j = 0; j < Count; ++j)
                {
                    // Don't check vertices on the current edge.
                    if (j == i1 || j == i2)
                    {
                        continue;
                    }

                    Vector2 r = this[j] - this[i1];

                    float s = edge.X * r.Y - edge.Y * r.X;

                    if (s <= 0.0f)
                        return false;
                }
            }
            return true;
        }

        public bool IsCounterClockWise()
        {
            //We just return true for lines
            if (Count < 3)
                return true;

            return (GetSignedArea() > 0.0f);
        }

        /// <summary>
        /// Forces counter clock wise order.
        /// </summary>
        public void ForceCounterClockWise()
        {
            if (!IsCounterClockWise())
            {
                Reverse();
            }
        }

        /// <summary>
        /// Check for edge crossings
        /// </summary>
        /// <returns></returns>
        public bool IsSimple()
        {
            for (int i = 0; i < Count; ++i)
            {
                int iplus = (i + 1 > Count - 1) ? 0 : i + 1;
                Vector2 a1 = new Vector2(this[i].X, this[i].Y);
                Vector2 a2 = new Vector2(this[iplus].X, this[iplus].Y);
                for (int j = i + 1; j < Count; ++j)
                {
                    int jplus = (j + 1 > Count - 1) ? 0 : j + 1;
                    Vector2 b1 = new Vector2(this[j].X, this[j].Y);
                    Vector2 b2 = new Vector2(this[jplus].X, this[jplus].Y);

                    Vector2 temp;

                    if (LineTools.LineIntersect2(a1, a2, b1, b2, out temp))
                    {
                        return false;
                    }
                }
            }
            return true;
        }

        //TODO: Test
        //Implementation found here: http://www.gamedev.net/community/forums/topic.asp?topic_id=548477
        public bool IsSimple2()
        {
            for (int i = 0; i < Count; ++i)
            {
                if (i < Count - 1)
                {
                    for (int h = i + 1; h < Count; ++h)
                    {
                        // Do two vertices lie on top of one another?
                        if (this[i] == this[h])
                        {
                            return true;
                        }
                    }
                }

                int j = (i + 1) % Count;
                Vector2 iToj = this[j] - this[i];
                Vector2 iTojNormal = new Vector2(iToj.Y, -iToj.X);

                // i is the first vertex and j is the second
                int startK = (j + 1) % Count;
                int endK = (i - 1 + Count) % Count;
                endK += startK < endK ? 0 : startK + 1;
                int k = startK;
                Vector2 iTok = this[k] - this[i];
                bool onLeftSide = Vector2.Dot(iTok, iTojNormal) >= 0;
                Vector2 prevK = this[k];
                ++k;
                for (; k <= endK; ++k)
                {
                    int modK = k % Count;
                    iTok = this[modK] - this[i];
                    if (onLeftSide != Vector2.Dot(iTok, iTojNormal) >= 0)
                    {
                        Vector2 prevKtoK = this[modK] - prevK;
                        Vector2 prevKtoKNormal = new Vector2(prevKtoK.Y, -prevKtoK.X);
                        if ((Vector2.Dot(this[i] - prevK, prevKtoKNormal) >= 0) !=
                            (Vector2.Dot(this[j] - prevK, prevKtoKNormal) >= 0))
                        {
                            return true;
                        }
                    }
                    onLeftSide = Vector2.Dot(iTok, iTojNormal) > 0;
                    prevK = this[modK];
                }
            }
            return false;
        }

        // From Eric Jordan's convex decomposition library

        /// <summary>
        /// Checks if polygon is valid for use in Box2d engine.
        /// Last ditch effort to ensure no invalid polygons are
        /// added to world geometry.
        ///
        /// Performs a full check, for simplicity, convexity,
        /// orientation, minimum angle, and volume.  This won't
        /// be very efficient, and a lot of it is redundant when
        /// other tools in this section are used.
        /// </summary>
        /// <returns></returns>
        public bool CheckPolygon()
        {
            int error = -1;
            if (Count < 3 || Count > Settings.MaxPolygonVertices)
            {
                error = 0;
            }
            if (!IsConvex())
            {
                error = 1;
            }
            if (!IsSimple())
            {
                error = 2;
            }
            if (GetArea() < Settings.Epsilon)
            {
                error = 3;
            }

            //Compute normals
            Vector2[] normals = new Vector2[Count];
            Vertices vertices = new Vertices(Count);
            for (int i = 0; i < Count; ++i)
            {
                vertices.Add(new Vector2(this[i].X, this[i].Y));
                int i1 = i;
                int i2 = i + 1 < Count ? i + 1 : 0;
                Vector2 edge = new Vector2(this[i2].X - this[i1].X, this[i2].Y - this[i1].Y);
                normals[i] = MathUtils.Cross(edge, 1.0f);
                normals[i].Normalize();
            }

            //Required side checks
            for (int i = 0; i < Count; ++i)
            {
                int iminus = (i == 0) ? Count - 1 : i - 1;

                //Parallel sides check
                float cross = MathUtils.Cross(normals[iminus], normals[i]);
                cross = MathUtils.Clamp(cross, -1.0f, 1.0f);
                float angle = (float)Math.Asin(cross);
                if (angle <= Settings.AngularSlop)
                {
                    error = 4;
                    break;
                }

                //Too skinny check
                for (int j = 0; j < Count; ++j)
                {
                    if (j == i || j == (i + 1) % Count)
                    {
                        continue;
                    }
                    float s = Vector2.Dot(normals[i], vertices[j] - vertices[i]);
                    if (s >= -Settings.LinearSlop)
                    {
                        error = 5;
                    }
                }


                Vector2 centroid = vertices.GetCentroid();
                Vector2 n1 = normals[iminus];
                Vector2 n2 = normals[i];
                Vector2 v = vertices[i] - centroid;

                Vector2 d = new Vector2();
                d.X = Vector2.Dot(n1, v); // - toiSlop;
                d.Y = Vector2.Dot(n2, v); // - toiSlop;

                // Shifting the edge inward by toiSlop should
                // not cause the plane to pass the centroid.
                if ((d.X < 0.0f) || (d.Y < 0.0f))
                {
                    error = 6;
                }
            }

            if (error != -1)
            {
                Debug.WriteLine("Found invalid polygon, ");
                switch (error)
                {
                    case 0:
                        Debug.WriteLine(string.Format("must have between 3 and {0} vertices.\n",
                                                      Settings.MaxPolygonVertices));
                        break;
                    case 1:
                        Debug.WriteLine("must be convex.\n");
                        break;
                    case 2:
                        Debug.WriteLine("must be simple (cannot intersect itself).\n");
                        break;
                    case 3:
                        Debug.WriteLine("area is too small.\n");
                        break;
                    case 4:
                        Debug.WriteLine("sides are too close to parallel.\n");
                        break;
                    case 5:
                        Debug.WriteLine("polygon is too thin.\n");
                        break;
                    case 6:
                        Debug.WriteLine("core shape generation would move edge past centroid (too thin).\n");
                        break;
                    default:
                        Debug.WriteLine("don't know why.\n");
                        break;
                }
            }
            return error != -1;
        }

        // From Eric Jordan's convex decomposition library

        /// <summary>
        /// Trace the edge of a non-simple polygon and return a simple polygon.
        /// 
        /// Method:
        /// Start at vertex with minimum y (pick maximum x one if there are two).
        /// We aim our "lastDir" vector at (1.0, 0)
        /// We look at the two rays going off from our start vertex, and follow whichever
        /// has the smallest angle (in -Pi . Pi) wrt lastDir ("rightest" turn)
        /// Loop until we hit starting vertex:
        /// We add our current vertex to the list.
        /// We check the seg from current vertex to next vertex for intersections
        /// - if no intersections, follow to next vertex and continue
        /// - if intersections, pick one with minimum distance
        /// - if more than one, pick one with "rightest" next point (two possibilities for each)
        /// </summary>
        /// <param name="verts">The vertices.</param>
        /// <returns></returns>
        public Vertices TraceEdge(Vertices verts)
        {
            PolyNode[] nodes = new PolyNode[verts.Count * verts.Count];
            //overkill, but sufficient (order of mag. is right)
            int nNodes = 0;

            //Add base nodes (raw outline)
            for (int i = 0; i < verts.Count; ++i)
            {
                Vector2 pos = new Vector2(verts[i].X, verts[i].Y);
                nodes[i].Position = pos;
                ++nNodes;
                int iplus = (i == verts.Count - 1) ? 0 : i + 1;
                int iminus = (i == 0) ? verts.Count - 1 : i - 1;
                nodes[i].AddConnection(nodes[iplus]);
                nodes[i].AddConnection(nodes[iminus]);
            }

            //Process intersection nodes - horribly inefficient
            bool dirty = true;
            int counter = 0;
            while (dirty)
            {
                dirty = false;
                for (int i = 0; i < nNodes; ++i)
                {
                    for (int j = 0; j < nodes[i].NConnected; ++j)
                    {
                        for (int k = 0; k < nNodes; ++k)
                        {
                            if (k == i || nodes[k] == nodes[i].Connected[j]) continue;
                            for (int l = 0; l < nodes[k].NConnected; ++l)
                            {
                                if (nodes[k].Connected[l] == nodes[i].Connected[j] ||
                                    nodes[k].Connected[l] == nodes[i]) continue;

                                //Check intersection
                                Vector2 intersectPt;

                                bool crosses = LineTools.LineIntersect(nodes[i].Position, nodes[i].Connected[j].Position,
                                                                       nodes[k].Position, nodes[k].Connected[l].Position,
                                                                       out intersectPt);
                                if (crosses)
                                {
                                    dirty = true;
                                    //Destroy and re-hook connections at crossing point
                                    PolyNode connj = nodes[i].Connected[j];
                                    PolyNode connl = nodes[k].Connected[l];
                                    nodes[i].Connected[j].RemoveConnection(nodes[i]);
                                    nodes[i].RemoveConnection(connj);
                                    nodes[k].Connected[l].RemoveConnection(nodes[k]);
                                    nodes[k].RemoveConnection(connl);
                                    nodes[nNodes] = new PolyNode(intersectPt);
                                    nodes[nNodes].AddConnection(nodes[i]);
                                    nodes[i].AddConnection(nodes[nNodes]);
                                    nodes[nNodes].AddConnection(nodes[k]);
                                    nodes[k].AddConnection(nodes[nNodes]);
                                    nodes[nNodes].AddConnection(connj);
                                    connj.AddConnection(nodes[nNodes]);
                                    nodes[nNodes].AddConnection(connl);
                                    connl.AddConnection(nodes[nNodes]);
                                    ++nNodes;
                                    goto SkipOut;
                                }
                            }
                        }
                    }
                }
            SkipOut:
                ++counter;
            }

            //Collapse duplicate points
            bool foundDupe = true;
            int nActive = nNodes;
            while (foundDupe)
            {
                foundDupe = false;
                for (int i = 0; i < nNodes; ++i)
                {
                    if (nodes[i].NConnected == 0) continue;
                    for (int j = i + 1; j < nNodes; ++j)
                    {
                        if (nodes[j].NConnected == 0) continue;
                        Vector2 diff = nodes[i].Position - nodes[j].Position;
                        if (diff.LengthSquared() <= Settings.Epsilon * Settings.Epsilon)
                        {
                            if (nActive <= 3)
                                return new Vertices();

                            //printf("Found dupe, %d left\n",nActive);
                            --nActive;
                            foundDupe = true;
                            PolyNode inode = nodes[i];
                            PolyNode jnode = nodes[j];
                            //Move all of j's connections to i, and orphan j
                            int njConn = jnode.NConnected;
                            for (int k = 0; k < njConn; ++k)
                            {
                                PolyNode knode = jnode.Connected[k];
                                Debug.Assert(knode != jnode);
                                if (knode != inode)
                                {
                                    inode.AddConnection(knode);
                                    knode.AddConnection(inode);
                                }
                                knode.RemoveConnection(jnode);
                            }
                            jnode.NConnected = 0;
                        }
                    }
                }
            }

            //Now walk the edge of the list

            //Find node with minimum y value (max x if equal)
            float minY = float.MaxValue;
            float maxX = -float.MaxValue;
            int minYIndex = -1;
            for (int i = 0; i < nNodes; ++i)
            {
                if (nodes[i].Position.Y < minY && nodes[i].NConnected > 1)
                {
                    minY = nodes[i].Position.Y;
                    minYIndex = i;
                    maxX = nodes[i].Position.X;
                }
                else if (nodes[i].Position.Y == minY && nodes[i].Position.X > maxX && nodes[i].NConnected > 1)
                {
                    minYIndex = i;
                    maxX = nodes[i].Position.X;
                }
            }

            Vector2 origDir = new Vector2(1.0f, 0.0f);
            Vector2[] resultVecs = new Vector2[4 * nNodes];
            // nodes may be visited more than once, unfortunately - change to growable array!
            int nResultVecs = 0;
            PolyNode currentNode = nodes[minYIndex];
            PolyNode startNode = currentNode;
            Debug.Assert(currentNode.NConnected > 0);
            PolyNode nextNode = currentNode.GetRightestConnection(origDir);
            if (nextNode == null)
            {
                Vertices vertices = new Vertices(nResultVecs);

                for (int i = 0; i < nResultVecs; ++i)
                {
                    vertices.Add(resultVecs[i]);
                }

                return vertices;
            }

            // Borked, clean up our mess and return
            resultVecs[0] = startNode.Position;
            ++nResultVecs;
            while (nextNode != startNode)
            {
                if (nResultVecs > 4 * nNodes)
                {
                    Debug.Assert(false);
                }
                resultVecs[nResultVecs++] = nextNode.Position;
                PolyNode oldNode = currentNode;
                currentNode = nextNode;
                nextNode = currentNode.GetRightestConnection(oldNode);
                if (nextNode == null)
                {
                    Vertices vertices = new Vertices(nResultVecs);
                    for (int i = 0; i < nResultVecs; ++i)
                    {
                        vertices.Add(resultVecs[i]);
                    }
                    return vertices;
                }
                // There was a problem, so jump out of the loop and use whatever garbage we've generated so far
            }

            return new Vertices();
        }

        private class PolyNode
        {
            private const int MaxConnected = 32;

            /*
             * Given sines and cosines, tells if A's angle is less than B's on -Pi, Pi
             * (in other words, is A "righter" than B)
             */
            public PolyNode[] Connected = new PolyNode[MaxConnected];
            public int NConnected;
            public Vector2 Position;

            public PolyNode(Vector2 pos)
            {
                Position = pos;
                NConnected = 0;
            }

            private bool IsRighter(float sinA, float cosA, float sinB, float cosB)
            {
                if (sinA < 0)
                {
                    if (sinB > 0 || cosA <= cosB) return true;
                    else return false;
                }
                else
                {
                    if (sinB < 0 || cosA <= cosB) return false;
                    else return true;
                }
            }

            public void AddConnection(PolyNode toMe)
            {
                Debug.Assert(NConnected < MaxConnected);

                // Ignore duplicate additions
                for (int i = 0; i < NConnected; ++i)
                {
                    if (Connected[i] == toMe) return;
                }
                Connected[NConnected] = toMe;
                ++NConnected;
            }

            public void RemoveConnection(PolyNode fromMe)
            {
                bool isFound = false;
                int foundIndex = -1;
                for (int i = 0; i < NConnected; ++i)
                {
                    if (fromMe == Connected[i])
                    {
                        //.position == connected[i].position){
                        isFound = true;
                        foundIndex = i;
                        break;
                    }
                }
                Debug.Assert(isFound);
                --NConnected;
                for (int i = foundIndex; i < NConnected; ++i)
                {
                    Connected[i] = Connected[i + 1];
                }
            }

            public PolyNode GetRightestConnection(PolyNode incoming)
            {
                if (NConnected == 0) Debug.Assert(false); // This means the connection graph is inconsistent
                if (NConnected == 1)
                {
                    //b2Assert(false);
                    // Because of the possibility of collapsing nearby points,
                    // we may end up with "spider legs" dangling off of a region.
                    // The correct behavior here is to turn around.
                    return incoming;
                }
                Vector2 inDir = Position - incoming.Position;

                float inLength = inDir.Length();
                inDir.Normalize();

                Debug.Assert(inLength > Settings.Epsilon);

                PolyNode result = null;
                for (int i = 0; i < NConnected; ++i)
                {
                    if (Connected[i] == incoming) continue;
                    Vector2 testDir = Connected[i].Position - Position;
                    float testLengthSqr = testDir.LengthSquared();
                    testDir.Normalize();
                    Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon);
                    float myCos = Vector2.Dot(inDir, testDir);
                    float mySin = MathUtils.Cross(inDir, testDir);
                    if (result != null)
                    {
                        Vector2 resultDir = result.Position - Position;
                        resultDir.Normalize();
                        float resCos = Vector2.Dot(inDir, resultDir);
                        float resSin = MathUtils.Cross(inDir, resultDir);
                        if (IsRighter(mySin, myCos, resSin, resCos))
                        {
                            result = Connected[i];
                        }
                    }
                    else
                    {
                        result = Connected[i];
                    }
                }

                Debug.Assert(result != null);

                return result;
            }

            public PolyNode GetRightestConnection(Vector2 incomingDir)
            {
                Vector2 diff = Position - incomingDir;
                PolyNode temp = new PolyNode(diff);
                PolyNode res = GetRightestConnection(temp);
                Debug.Assert(res != null);
                return res;
            }
        }

        public override string ToString()
        {
            StringBuilder builder = new StringBuilder();
            for (int i = 0; i < Count; i++)
            {
                builder.Append(this[i].ToString());
                if (i < Count - 1)
                {
                    builder.Append(" ");
                }
            }
            return builder.ToString();
        }

        /// <summary>
        /// Projects to axis.
        /// </summary>
        /// <param name="axis">The axis.</param>
        /// <param name="min">The min.</param>
        /// <param name="max">The max.</param>
        public void ProjectToAxis(ref Vector2 axis, out float min, out float max)
        {
            // To project a point on an axis use the dot product
            float dotProduct = Vector2.Dot(axis, this[0]);
            min = dotProduct;
            max = dotProduct;

            for (int i = 0; i < Count; i++)
            {
                dotProduct = Vector2.Dot(this[i], axis);
                if (dotProduct < min)
                {
                    min = dotProduct;
                }
                else
                {
                    if (dotProduct > max)
                    {
                        max = dotProduct;
                    }
                }
            }
        }

        /// <summary>
        /// Winding number test for a point in a polygon.
        /// </summary>
        /// See more info about the algorithm here: http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm
        /// <param name="point">The point to be tested.</param>
        /// <returns>-1 if the winding number is zero and the point is outside
        /// the polygon, 1 if the point is inside the polygon, and 0 if the point
        /// is on the polygons edge.</returns>
        public int PointInPolygon(ref Vector2 point)
        {
            // Winding number
            int wn = 0;

            // Iterate through polygon's edges
            for (int i = 0; i < Count; i++)
            {
                // Get points
                Vector2 p1 = this[i];
                Vector2 p2 = this[NextIndex(i)];

                // Test if a point is directly on the edge
                Vector2 edge = p2 - p1;
                float area = MathUtils.Area(ref p1, ref p2, ref point);
                if (area == 0f && Vector2.Dot(point - p1, edge) >= 0f && Vector2.Dot(point - p2, edge) <= 0f)
                {
                    return 0;
                }
                // Test edge for intersection with ray from point
                if (p1.Y <= point.Y)
                {
                    if (p2.Y > point.Y && area > 0f)
                    {
                        ++wn;
                    }
                }
                else
                {
                    if (p2.Y <= point.Y && area < 0f)
                    {
                        --wn;
                    }
                }
            }
            return (wn == 0 ? -1 : 1);
        }

        /// <summary>
        /// Compute the sum of the angles made between the test point and each pair of points making up the polygon. 
        /// If this sum is 2pi then the point is an interior point, if 0 then the point is an exterior point. 
        /// ref: http://ozviz.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/  - Solution 2 
        /// </summary>
        public bool PointInPolygonAngle(ref Vector2 point)
        {
            double angle = 0;

            // Iterate through polygon's edges
            for (int i = 0; i < Count; i++)
            {
                // Get points
                Vector2 p1 = this[i] - point;
                Vector2 p2 = this[NextIndex(i)] - point;

                angle += MathUtils.VectorAngle(ref p1, ref p2);
            }

            if (Math.Abs(angle) < Math.PI)
            {
                return false;
            }

            return true;
        }
    }
}