A星算法

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;

public class AStar
{

    public static PriorityQueue closedList;
    public static PriorityQueue openList;



    public static List<Node> FindPath(Node start, Node goal)
    {
        closedList = new PriorityQueue();
        openList = new PriorityQueue();         //初始化OpenList
        openList.Push(start);                           //把开始节点放入OpenList中

        start.F = 0;
        start.G = 0;
        start.H = NodeCost(start, goal);

        Node node = null;
        List<Node> neighbours = null;
        Node neighbourNode = null;
        DateTime now = System.DateTime.Now;

        while (openList.Length != 0)
        {
            node = openList.First();

            //如果是终点,就返回一个列表list过去
            if (node.x == goal.x && node.y == goal.y)
            {
                Debug.Log("算法所消耗的时间: " + (System.DateTime.Now - now).TotalSeconds);
                return CalculatePath(node);
            }

            neighbours = new List<Node>();
            GridManager.Instance.GetNeighbours(node, neighbours);           //获取周围的

            //遍历周围的方块
            for (int i = 0; i < neighbours.Count; i++)
            {
                neighbourNode = neighbours[i];

                //如果当前方块不在关闭列表中
                if (!closedList.Contains(neighbourNode))
                {
                    neighbourNode.G = node.G + 1;                                                   //是上一个格子的G+1(这里并没有让斜角加1.4)  
                    neighbourNode.H = NodeCost(neighbourNode, goal);                                //这个格子离终点的估算
                    neighbourNode.F = neighbourNode.G + neighbourNode.H;

                    //显示H F G的值
                    neighbourNode.SetH(neighbourNode.H);
                    neighbourNode.SetF(neighbourNode.F);
                    neighbourNode.SetG(neighbourNode.G);

                    neighbourNode.parent = node;
                    //如果方块不在开启列表中,就添加到开启列表中
                    if (!openList.Contains(neighbourNode))
                    {
                        openList.Push(neighbourNode);
                    }
                }
            }




            //把寻找的节点放入关闭列表中
            closedList.Push(node);
            openList.Remove(node);
        }

        Debug.Log("算法所消耗的时间: " + (System.DateTime.Now - now).TotalMilliseconds);

        if (node.x != goal.x && node.y != goal.y)
        {
            Debug.LogError("没有找到路径");
            return null;
        }

        Debug.Log("找到路径了");
        return CalculatePath(node);
    }

    /// <summary>
    /// 获取两个节点的距离(不计算障碍物)
    /// </summary>
    /// <param name="a">开始节点</param>
    /// <param name="b">结束节点</param>
    /// <returns>返回x +y 的距离</returns>
    private static int NodeCost(Node a, Node b)
    {
        int x = (int)Mathf.Abs(b.x - a.x);
        int y = (int)Mathf.Abs(b.y - a.y);
        return x + y;
    }

    /// <summary>
    /// 获取最终路径
    /// </summary>
    /// <param name="node"></param>
    /// <returns></returns>
    private static List<Node> CalculatePath(Node node)
    {
        List<Node> list = new List<Node>();

        while (node != null)
        {
            list.Add(node);
            node = node.parent;
        }
        list.Reverse();
        return list;
    }

}