private static void SaveNavMeshLinks(NavMesh navMesh, int indentLevel, StringBuilder xmlData)
        {
            xmlData.AppendLine(new string('\t', indentLevel) + "<links>");
            {
                indentLevel++;
                foreach (var link in navMesh.Links)
                {
                    xmlData.AppendLine(new string('\t', indentLevel) + "<link>");
                    {
                        indentLevel++;

                        xmlData.AppendFormat(new string('\t', indentLevel) +
                            "<start polygon=\"{0}\" edgestart=\"{1}\" edgeend=\"{2}\"/>\n",
                            FindPolygonIndex(navMesh, link.StartPoly),
                            link.StartEdgeIndex.Start,
                            link.StartEdgeIndex.End);

                        xmlData.AppendFormat(new string('\t', indentLevel) +
                            "<end polygon=\"{0}\" edgestart=\"{1}\" edgeend=\"{2}\"/>\n",
                            FindPolygonIndex(navMesh, link.EndPoly),
                            link.EndEdgeIndex.Start,
                            link.EndEdgeIndex.End);

                        indentLevel--;
                    }
                    xmlData.AppendLine(new string('\t', indentLevel) + "</link>");
                }
                indentLevel--;
            }
            xmlData.AppendLine(new string('\t', indentLevel) + "</links>");
        }
        /// <summary>
        /// Works out a graph of nodes on top of the navmesh.
        /// Each polygon is a node and a node is added a connecting edges of linked polygons.
        /// The node graph class member is filled in by running this method.
        /// It's quite likely this code isn't very optimal.
        /// </summary>
        /// <param name="polyStart">The polygon the entity is currently in.</param>
        /// <param name="polyEnd">The polygon the entity would like to be in.</param>
        /// <param name="from">The point in the entity is current positioned.</param>
        /// <param name="to">The point the entity would like to be positioned.</param>
        /// <param name="navMesh">The navigation mesh describing the polygons and their links.</param>
        /// <returns>The start and end nodes for the journey the player wants to take</returns>
        private Tuple<NavigationNode, NavigationNode> CreateNodeNetwork(ConvexPolygon polyStart, ConvexPolygon polyEnd, Point from, Point to, NavMesh navMesh)
        {
            _nodeGraph.Clear();
            // Store a map poly -> node to make it simple to work out the connection nodes.
            Dictionary<ConvexPolygon, NavigationNode> polyToNodeMap = new Dictionary<ConvexPolygon, NavigationNode>();

            NavigationNode startNode = null;
            NavigationNode endNode = null;

            // Create a node for the centroid of each polygon
            // Replace the postion of the start and end polygon.
            foreach (ConvexPolygon polygon in navMesh.PolygonList)
            {
                Point position;
                NavigationNode node;
                if (polyStart == polygon)
                {
                    position = from;
                    node = new NavigationNode(position);
                    startNode = node;
                }
                else if (polyEnd == polygon)
                {
                    position = to;
                    node = new NavigationNode(position);
                    endNode = node;
                }
                else
                {
                    position = polygon.CalculateCentroid();
                    node = new NavigationNode(position);
                }


                _nodeGraph.Add(node);
                polyToNodeMap.Add(polygon, node);
            }

            // Create the edge nodes and add the links
            // !* This is where you'd add several nodes per edge, if you wanted.
            foreach (PolygonLink link in navMesh.Links)
            {
                LineSegment line = link.GetShortestEdge();
                Point midPoint = line.GetMiddle();

                NavigationNode connectionNode = new NavigationNode(midPoint);

                // Add bidirectional links to connected polys and edge polys.
                polyToNodeMap[link.StartPoly].Neighbours.Add(connectionNode);
                connectionNode.Neighbours.Add(polyToNodeMap[link.StartPoly]);

                polyToNodeMap[link.EndPoly].Neighbours.Add(connectionNode);
                connectionNode.Neighbours.Add(polyToNodeMap[link.EndPoly]);

                _nodeGraph.Add(connectionNode);
            }

            return new Tuple<NavigationNode, NavigationNode>(startNode, endNode);
        }
        public List<Point> GetPath(Point from, Point to, NavMesh navMesh)
        {
            List<Point> path = new List<Point>();
            // First find the polygon they're in 
            // !* May want a little overlapping so all screen can be clicked.
            //    In that case this should check if all intersected options contain as same poly start/end
            ConvexPolygon polyStart = navMesh.PolygonList.First(x => x.Intersects(from));
            ConvexPolygon polyEnd = navMesh.PolygonList.First(x => x.Intersects(to));

            if (polyStart == null || polyEnd == null)
            {
                return path;
            }
            else if (polyStart == polyEnd)
            {
                path.Add(from);
                path.Add(to);
            }
            else if (polyStart != polyEnd)
            {
                // This does not need doing every time but it's easier to code if is recreated.
                _astar = new AStar<NavigationNode>(

                 delegate(NavigationNode startNode, NavigationNode endNode)
                 {
                     return Math.Sqrt(startNode.Position.X * endNode.Position.X
                                    + startNode.Position.Y * endNode.Position.Y);
                 });


                var startEndNodes = CreateNodeNetwork(polyStart, polyEnd, from, to, navMesh);
                _astar.FindPath(startEndNodes.Item1, startEndNodes.Item2);
                _astar.Path.Reverse();
                foreach (var node in _astar.Path)
                {
                    path.Add(node.Position);
                }
            }


            return path;
        }
 private static int FindPolygonIndex(NavMesh navMesh, ConvexPolygon convexPolygon)
 {
     return navMesh.PolygonList.FindIndex(convexPolygon.Equals);
 }
 private static void LoadSinglePolygon(XmlTextReader xmlReader, NavMesh navMesh)
 {
     ConvexPolygon polygon = new ConvexPolygon();
     xmlReader.MoveToContent();
     while (xmlReader.Read())
     {
         if ("point" == xmlReader.Name)
         {
             float x = float.Parse(xmlReader.GetAttribute("x"));
             float y = float.Parse(xmlReader.GetAttribute("y"));
             polygon.Vertices.Add(new Point(x, y));
         }
         else if("polygon" == xmlReader.Name)
         {
             polygon.GenerateEdges();
             navMesh.AddPolygon(polygon);
             return;
         }
     }
 }
 private static void LoadPolygons(XmlTextReader xmlReader, NavMesh navMesh)
 {
       xmlReader.MoveToContent();
       while (xmlReader.Read())
       {
           if ("polygon" == xmlReader.Name)
           {
               LoadSinglePolygon(xmlReader, navMesh);
           }
           else if ("polygons" == xmlReader.Name)
           {
               return;
           }
       }
 }
        /// <summary>
        /// This function will look up the polygon index right away. 
        /// This could potentially cause problems if the XML has the polygon defs later in the file.
        /// The values should really be cached and it should be done after.
        /// (or thrown in a closure but the C# syntax is a little too messy for that)
        /// </summary>
        /// <param name="xmlReader"></param>
        /// <param name="navMesh"></param>
        private static void LoadSingleLink(XmlTextReader xmlReader, NavMesh navMesh)
        {
            int startPolygonIndex   = -1;
            int startEdgeStart      = -1;
            int startEdgeEnd        = -1;

            int endPolygonIndex = -1;
            int endEdgeStart    = -1;
            int endEdgeEnd      = -1;

            xmlReader.MoveToContent();
            while (xmlReader.Read())
            {
                if ("start" == xmlReader.Name)
                {
                    startPolygonIndex = int.Parse(xmlReader.GetAttribute("polygon"));
                    startEdgeStart = int.Parse(xmlReader.GetAttribute("edgestart"));
                    startEdgeEnd = int.Parse(xmlReader.GetAttribute("edgeend"));   
                }
                else if ("end" == xmlReader.Name)
                {
                    endPolygonIndex = int.Parse(xmlReader.GetAttribute("polygon"));
                    endEdgeStart = int.Parse(xmlReader.GetAttribute("edgestart"));
                    endEdgeEnd = int.Parse(xmlReader.GetAttribute("edgeend"));
                }
                else if ("link" == xmlReader.Name)
                {
                    PolygonLink polygonLink = new PolygonLink(
                        navMesh.PolygonList[startPolygonIndex],
                        new IndexedEdge(startEdgeStart, startEdgeEnd),
                        navMesh.PolygonList[endPolygonIndex],
                        new IndexedEdge(endEdgeStart, endEdgeEnd));
                    navMesh.AddLink(polygonLink);
                    return;
                }
            }
        }
 private static void LoadLinks(XmlTextReader xmlReader, NavMesh navMesh)
 {
     
     xmlReader.MoveToContent();
     while (xmlReader.Read())
     {
         if ("link" == xmlReader.Name)
         {
             LoadSingleLink(xmlReader, navMesh);
         }
         else if ("links" == xmlReader.Name)
         {
             return;
         }
     }
 }
 private static void SaveNavMeshPolygons(NavMesh navMesh, int indentLevel, StringBuilder xmlData)
 {
     xmlData.AppendLine(new string('\t', indentLevel) + "<polygons>");
     {
         indentLevel++;
         foreach (ConvexPolygon polygon in navMesh.PolygonList)
         {
             xmlData.AppendLine(new string('\t', indentLevel) + "<polygon>");
             {
                 indentLevel++;
                 foreach (Point point in polygon.Vertices)
                 {
                     xmlData.AppendFormat(new string('\t', indentLevel) +
                         "<point x=\"{0}\" y =\"{1}\"/>",
                         point.X, point.Y);
                 }
                 indentLevel--;
             }
             xmlData.AppendLine(new string('\t', indentLevel) + "</polygon>");
         }
         indentLevel--;
     }
     xmlData.AppendLine(new string('\t', indentLevel) + "</polygons>");
 }
 private void RenderNavMesh(NavMesh navMesh)
 {
     GLUtil.SetColor(new Color(0f, 0f, 1.0f, 0.5f));
     navMesh.PolygonList.ForEach(x => GLUtil.RenderPolygonFilled(x));            
     GLUtil.SetColor(new Color(0.5f, 0.1f, 0.1f, 1f));
     navMesh.PolygonList.ForEach(x => GLUtil.RenderPolygon(x));
     
 }
 public AddPolygonState(Input input, NavMesh navMesh)
 {
     _input = input;
     _navMesh = navMesh;
 }
 public DefaultEditState(Input input, NavMesh navMesh)
 {
     _input = input;
     _navMesh = navMesh;
 }
 public AddLinkState(Input input, NavMesh navMesh)
 {
     _input = input;
     _navMesh = navMesh;
 }
 public AddVertexState(Input input, NavMesh navMesh)
 {
     _input = input;
     _navMesh = navMesh;
 }