Beispiel #1
0
    // Find the path between start node and goal node using AStar Algorithm
    public static ArrayList FindPath(EnemyNutrientNode start, EnemyNutrientNode goal)
    {
        // Start Finding the path
        frontier    = new Frontier_ExploredSet();                               // Initialize the frontier
        exploredSet = new Frontier_ExploredSet();                               // Initialize the explored set
        frontier.Push(start);                                                   // Add the start node to the frontier
        start.fTotalCost     = 0.0f;                                            // Initialize the total cost for the start node
        start.fEstimatedCost = HeuristicEstimateCost(start, goal);              // Calculate the heuristic

        EnemyNutrientNode node = null;                                          // Initialize a node to use

        // While the frontier isnot empty
        while (frontier.Length != 0)
        {
            node = frontier.First();                                                                            // Take the first node in the frontier

            // If the node is the goal node, then the path is found
            // Instead of checking after adding the node to the explored set, we check it when it's first generated
            // The purpose is to save time from looking for a more optimal path
            if (node.position == goal.position)
            {
                return(CalculatePath(node));
            }

            ArrayList neighbours = new ArrayList();
            MapManager.Instance.GetNeighbours(node, neighbours);

            #region CheckNeighbours

            // Get the Neighbours
            for (int i = 0; i < neighbours.Count; i++)
            {
                // Cost between neighbour nodes
                EnemyNutrientNode neighbourNode = (EnemyNutrientNode)neighbours[i];

                if (!exploredSet.Contains(neighbourNode))
                {
                    // Cost from current node to this neighbour node
                    float cost = HeuristicEstimateCost(node, neighbourNode);

                    // Total Cost So Far from start to this neighbour node
                    float totalCost = node.fTotalCost + cost;

                    // Estimated cost for neighbour node to the goal
                    float neighbourNodeEstCost = HeuristicEstimateCost(neighbourNode, goal);

                    // Assign neighbour node properties
                    neighbourNode.fTotalCost     = totalCost;
                    neighbourNode.parent         = node;
                    neighbourNode.fEstimatedCost = totalCost + neighbourNodeEstCost;

                    // Add the neighbour node to the frontier if not already existed in the frontier
                    if (!frontier.Contains(neighbourNode))
                    {
                        frontier.Push(neighbourNode);
                    }
                }
            }

            #endregion

            exploredSet.Push(node);                                                                             // Add the node to the explored set as it is expanded
            frontier.Remove(node);                                                                              // Remove the node from the frontier
        }

        // If finished looping and cannot find the goal then return null
        if (node.position != goal.position)
        {
            //Debug.LogError("Goal Not Found");
            //Debug.Log ("Last node position:" + node.position);
            //Debug.Log ("Goal node position:" + goal.position);
            return(null);
        }

        // Calculate the path based on the final node
        return(CalculatePath(node));
    }
    // Find the path between start node and goal node using AStar Algorithm
    public static ArrayList FindPath(EnemyNutrientNode start, EnemyNutrientNode goal)
    {
        // Start Finding the path
        frontier = new Frontier_ExploredSet ();						// Initialize the frontier
        exploredSet = new Frontier_ExploredSet ();					// Initialize the explored set
        frontier.Push(start);										// Add the start node to the frontier
        start.fTotalCost = 0.0f;									// Initialize the total cost for the start node
        start.fEstimatedCost = HeuristicEstimateCost(start, goal);	// Calculate the heuristic

        EnemyNutrientNode node = null;								// Initialize a node to use

        // While the frontier isnot empty
        while (frontier.Length != 0)
        {
            node = frontier.First();								// Take the first node in the frontier

            // If the node is the goal node, then the path is found
            // Instead of checking after adding the node to the explored set, we check it when it's first generated
            // The purpose is to save time from looking for a more optimal path
            if (node.position == goal.position)
            {
                return CalculatePath(node);
            }

            ArrayList neighbours = new ArrayList();
            MapManager.Instance.GetNeighbours(node, neighbours);

            #region CheckNeighbours

            // Get the Neighbours
            for (int i = 0; i < neighbours.Count; i++)
            {
                // Cost between neighbour nodes
                EnemyNutrientNode neighbourNode = (EnemyNutrientNode)neighbours[i];

                if (!exploredSet.Contains(neighbourNode))
                {
                    // Cost from current node to this neighbour node
                    float cost = HeuristicEstimateCost(node, neighbourNode);

                    // Total Cost So Far from start to this neighbour node
                    float totalCost = node.fTotalCost + cost;

                    // Estimated cost for neighbour node to the goal
                    float neighbourNodeEstCost = HeuristicEstimateCost(neighbourNode, goal);

                    // Assign neighbour node properties
                    neighbourNode.fTotalCost = totalCost;
                    neighbourNode.parent = node;
                    neighbourNode.fEstimatedCost = totalCost + neighbourNodeEstCost;

                    // Add the neighbour node to the frontier if not already existed in the frontier
                    if (!frontier.Contains(neighbourNode))
                    {
                        frontier.Push(neighbourNode);
                    }
                }
            }

            #endregion

            exploredSet.Push(node);									// Add the node to the explored set as it is expanded
            frontier.Remove(node);									// Remove the node from the frontier
        }

        // If finished looping and cannot find the goal then return null
        if (node.position != goal.position)
        {
            //Debug.LogError("Goal Not Found");
            //Debug.Log ("Last node position:" + node.position);
            //Debug.Log ("Goal node position:" + goal.position);
            return null;
        }

        // Calculate the path based on the final node
        return CalculatePath(node);
    }