Ejemplo n.º 1
0
    /**
     * <summary>
     * Search the nodes from start to the goal
     * and get the paths
     * </summary>
     *
     * <param name="start">Node start</param>
     * <param name="goal">Node goal</param>
     *
     * <returns>
     * List<Node> paths
     * </returns>
     */
    public List <Node> Search(Node start, Node goal)
    {
        // Paths
        List <Node> paths = new List <Node>();

        // Priority queue
        BinaryMinHeap queue = new BinaryMinHeap();

        queue.Insert(start);

        //start.SetCameFrom(null);
        start.SetGScore(0);
        start.SetFScore(this.Hueristic(start, goal));

        // Filter out empty nodes
        List <Node> filtered = queue.GetNodes().FindAll((node) => { return(node != null); });

        while (filtered.Count > 0)
        {
            Node current = queue.Extract();

            // If the goal is found, get and return reconstructed paths
            if (current.GetID() == goal.GetID())
            {
                return(paths = this.ReconstructPath(current));
            }

            for (int i = 0; i < current.GetNeighbours().Count; i++)
            {
                Node neighbour = current.GetNeighbours()[i];

                // The cost of moving to the next neighbour
                float tentativeGScore = current.GetGScore() + this.MovementCost(current, neighbour);

                // if the new gScore is less than the neighbour gScore
                if (tentativeGScore < neighbour.GetGScore())
                {
                    // Set neighbour cameFrom to the current
                    neighbour.SetCameFrom(current);

                    // Set the neighbour gScore to the lower gScore
                    neighbour.SetGScore(tentativeGScore);

                    // Set the new FScore
                    neighbour.SetFScore(neighbour.GetGScore() + this.Hueristic(neighbour, goal));

                    bool exist = false;

                    // Is the neighbour in the open set or priority queue
                    for (int n = 0; n < queue.GetNodes().Count; n++)
                    {
                        Node node = queue.GetNodes()[n];

                        if (node != null && node.GetID() == neighbour.GetID())
                        {
                            exist = true;
                            break;
                        }
                    }

                    if (!exist)
                    {
                        queue.Insert(neighbour);
                    }
                }
            }
        }

        return(paths);
    }