Esempio n. 1
0
        private void InitilizeSearchNodes()
        {
            _searchNodes = new SearchNode[_mapBounds.Width, _mapBounds.Height];

            for (int x = 0; x < _mapBounds.Width; x++)
            {
                for (int y = 0; y < _mapBounds.Height; y++)
                {
                    var node = new SearchNode(new Vector(x, y))
                    {
                        Walkable = true
                    };

                    if (_layer.CheckCollision(new Vector(x * Settings.TileSize, y * Settings.TileSize), new Rect(0, 0, 32, 32)))
                    {
                        node.Walkable = false;
                    }

                    _searchNodes[x, y] = node;
                }
            }

            // Add all of the nodes to the SearchNode array.
            for (int x = 0; x < _searchNodes.GetLength(0); x++)
            {
                for (int y = 0; y < _searchNodes.GetLength(1); y++)
                {
                    if (_searchNodes[x, y] == null)
                    {
                        var node = new SearchNode(new Vector(x, y));
                        node.Walkable      = true;
                        _searchNodes[x, y] = node;
                    }
                }
            }



            // Loop back through and add the neighbors of the nodes.
            for (int x = 0; x < _searchNodes.GetLength(0); x++)
            {
                for (int y = 0; y < _searchNodes.GetLength(1); y++)
                {
                    SearchNode[] neighbors = new SearchNode[4];

                    Vector[] neighborPositions = new Vector[]
                    {
                        new Vector(x, y - 1),
                        new Vector(x, y + 1),
                        new Vector(x - 1, y),
                        new Vector(x + 1, y)
                    };

                    for (int i = 0; i < neighborPositions.Length; i++)
                    {
                        if (neighborPositions[i].X < 0 || neighborPositions[i].X >= _searchNodes.GetLength(0))
                        {
                            continue;
                        }

                        if (neighborPositions[i].Y < 0 || neighborPositions[i].Y >= _searchNodes.GetLength(1))
                        {
                            continue;
                        }

                        if (_searchNodes[(int)neighborPositions[i].X, (int)neighborPositions[i].Y].Walkable)
                        {
                            neighbors[i] = _searchNodes[(int)neighborPositions[i].X, (int)neighborPositions[i].Y];
                        }
                    }

                    _searchNodes[x, y].SetNeighbors(neighbors);
                }
            }
        }
Esempio n. 2
0
        /// <summary>
        /// Finds the optimal path from one point to another.
        /// </summary>
        public List <Vector> FindPath(Vector startPoint, Vector endPoint)
        {
            Vector normStartPoint = new Vector((int)(startPoint.X / Settings.TileSize), (int)(startPoint.Y / Settings.TileSize));
            Vector normEndPoint   = new Vector((int)(endPoint.X / Settings.TileSize), (int)(endPoint.Y / Settings.TileSize));

            if (normStartPoint.X < _mapBounds.Left || normStartPoint.Y < _mapBounds.Top ||
                normEndPoint.X < _mapBounds.Left || normEndPoint.Y < _mapBounds.Top)
            {
                return(new List <Vector>());
            }

            if (normStartPoint.X >= _mapBounds.Width || normStartPoint.Y >= _mapBounds.Height ||
                normEndPoint.X >= _mapBounds.Width || normEndPoint.Y >= _mapBounds.Height)
            {
                return(new List <Vector>());
            }

            // Only try to find a path if the start and end points are different.
            if (normStartPoint.X == normEndPoint.X && normStartPoint.Y == normEndPoint.Y)
            {
                return(new List <Vector>());
            }

            /////////////////////////////////////////////////////////////////////
            // Step 1 : Clear the Open and Closed Lists and reset each node’s F
            //          and G values in case they are still set from the last
            //          time we tried to find a path.
            /////////////////////////////////////////////////////////////////////
            ResetSearchNodes();

            // Store references to the start and end nodes for convenience.
            SearchNode startNode = _searchNodes[(int)normStartPoint.X, (int)normStartPoint.Y];
            SearchNode endNode   = _searchNodes[(int)normEndPoint.X, (int)normEndPoint.Y];

            /////////////////////////////////////////////////////////////////////
            // Step 2 : Set the start node’s G value to 0 and its F value to the
            //          estimated distance between the start node and goal node
            //          (this is where our H function comes in) and add it to the
            //          Open List.
            /////////////////////////////////////////////////////////////////////
            startNode.InOpenList = true;

            startNode.DistanceToGoal   = Heuristic(normStartPoint, normEndPoint);
            startNode.DistanceTraveled = 0;

            openList.Add(startNode);

            /////////////////////////////////////////////////////////////////////
            // Setp 3 : While there are still nodes to look at in the Open list :
            /////////////////////////////////////////////////////////////////////
            while (openList.Count > 0)
            {
                /////////////////////////////////////////////////////////////////
                // a) : Loop through the Open List and find the node that
                //      has the smallest F value.
                /////////////////////////////////////////////////////////////////
                SearchNode currentNode = FindBestNode();

                /////////////////////////////////////////////////////////////////
                // b) : If the Open List empty or no node can be found,
                //      no path can be found so the algorithm terminates.
                /////////////////////////////////////////////////////////////////
                if (currentNode == null)
                {
                    break;
                }

                /////////////////////////////////////////////////////////////////
                // c) : If the Active Node is the goal node, we will
                //      find and return the final path.
                /////////////////////////////////////////////////////////////////

                if (currentNode.Position.X == endNode.Position.X && currentNode.Position.Y == endNode.Position.Y)
                {
                    // Trace our path back to the start.
                    return(FindFinalPath(startNode, endNode));
                }

                /////////////////////////////////////////////////////////////////
                // d) : Else, for each of the Active Node’s neighbours :
                /////////////////////////////////////////////////////////////////
                for (int i = 0; i < currentNode.GetNeighbors().Length; i++)
                {
                    SearchNode neighbor = currentNode.GetNeighbors()[i];

                    //////////////////////////////////////////////////
                    // i) : Make sure that the neighbouring node can
                    //      be walked across.
                    //////////////////////////////////////////////////
                    if (neighbor == null || neighbor.Walkable == false)
                    {
                        continue;
                    }

                    //////////////////////////////////////////////////
                    // ii) Calculate a new G value for the neighbouring node.
                    //////////////////////////////////////////////////
                    float distanceTraveled = currentNode.DistanceTraveled + 1;

                    // An estimate of the distance from this node to the end node.
                    float heuristic = Heuristic(neighbor.Position, normEndPoint);

                    //////////////////////////////////////////////////
                    // iii) If the neighbouring node is not in either the Open
                    //      List or the Closed List :
                    //////////////////////////////////////////////////
                    if (neighbor.InOpenList == false && neighbor.InClosedList == false)
                    {
                        // (1) Set the neighbouring node’s G value to the G value
                        //     we just calculated.
                        neighbor.DistanceTraveled = distanceTraveled;
                        // (2) Set the neighbouring node’s F value to the new G value +
                        //     the estimated distance between the neighbouring node and
                        //     goal node.
                        neighbor.DistanceToGoal = distanceTraveled + heuristic;
                        // (3) Set the neighbouring node’s Parent property to point at the Active
                        //     Node.
                        neighbor.Parent = currentNode;
                        // (4) Add the neighbouring node to the Open List.
                        neighbor.InOpenList = true;
                        openList.Add(neighbor);
                    }
                    //////////////////////////////////////////////////
                    // iv) Else if the neighbouring node is in either the Open
                    //     List or the Closed List :
                    //////////////////////////////////////////////////
                    else if (neighbor.InOpenList || neighbor.InClosedList)
                    {
                        // (1) If our new G value is less than the neighbouring
                        //     node’s G value, we basically do exactly the same
                        //     steps as if the nodes are not in the Open and
                        //     Closed Lists except we do not need to add this node
                        //     the Open List again.
                        if (neighbor.DistanceTraveled > distanceTraveled)
                        {
                            neighbor.DistanceTraveled = distanceTraveled;
                            neighbor.DistanceToGoal   = distanceTraveled + heuristic;

                            neighbor.Parent = currentNode;
                        }
                    }
                }

                /////////////////////////////////////////////////////////////////
                // e) Remove the Active Node from the Open List and add it to the
                //    Closed List
                /////////////////////////////////////////////////////////////////
                openList.Remove(currentNode);
                currentNode.InClosedList = true;
            }

            // No path could be found.
            return(new List <Vector>());
        }