A*寻路 学习

/****************************************************
    文件：AStarNode.cs
	功能：A*节点类
*****************************************************/

using UnityEngine;

public class AStarNode
{
    public AStarNode Parent { get; set; }

    public int X { get; set; }

    public int Y { get; set; }

    public float F { get; set; }
    public float G { get; set; }
    public float H { get; set; }

    public bool NoPass { get; set; }

    public AStarNode(int x, int y, bool noPass, AStarNode parent = null)
    {
        this.X = x;
        this.Y = y;
        this.NoPass = noPass;
    }


    public void UpdateParent(AStarNode parent, float g)
    {
        this.Parent = parent;
        this.G = g;
        F = G + H;
    }
}

/****************************************************
    文件：AStarMgr.cs
	功能：AStar管理器
*****************************************************/

using UnityEngine;
using System.Collections.Generic;

public class AStarMgr
{
    private static AStarMgr _instance;

    public static AStarMgr Instance
    {
        get
        {
            if (_instance == null)
            {
                _instance = new AStarMgr();
            }
            return _instance;
        }
    }

    private int width;
    private int height;

    List<AStarNode> openList;
    List<AStarNode> closeList;

    public AStarNode[,] map;

    public void InitMap(int w, int h)
    {
        openList = new List<AStarNode>();
        closeList = new List<AStarNode>();

        width = w;
        height = h;

        map = new AStarNode[width, height];
        for (int x = 0; x < width; x++)
        {
            for (int y = 0; y < height; y++)
            {
                map[x, y] = new AStarNode(x, y, false);
            }
        }
        map[4, 2].NoPass = true;
        map[4, 3].NoPass = true;
        map[4, 4].NoPass = true;

    }

    public List<AStarNode> FindPath(AStarNode startPos, AStarNode endPos)
    {
        List<AStarNode> Temp = null;

        if (startPos == null || endPos == null)
        {
            Debug.LogWarning("不存在的起点或终点");
            return null;
        }
        if (map[startPos.X, startPos.Y] == null || map[endPos.X, endPos.Y] == null)
        {
            Debug.LogWarning("不在地图内的起点或终点");
            return null;
        }
        if (map[startPos.X, startPos.Y].NoPass || map[endPos.X, endPos.Y].NoPass)
        {
            Debug.LogWarning("不能到达的的起点或终点（障碍物）");
            return null;
        }

        //清空上次寻路的记录
        openList.Clear();
        closeList.Clear();

        openList.Add(startPos);
        CalcF(startPos, endPos);
        while (openList.Count > 0)
        {
            AStarNode minFPos = FindMinFPos(openList);
            openList.Remove(minFPos);
            closeList.Add(minFPos);

            List<AStarNode> surrentPoss = GetSurroundPoints(minFPos);
            Filter(surrentPoss, closeList);

            for (int i = 0; i < surrentPoss.Count; i++)
            {
                if (openList.IndexOf(surrentPoss[i]) > -1)
                {
                    //重新计算G值，比较大小
                    float nowG = CalcG(surrentPoss[i], minFPos);
                    if (nowG < surrentPoss[i].G)
                    {
                        surrentPoss[i].UpdateParent(minFPos, nowG);
                    }
                }
                else
                {
                    surrentPoss[i].Parent = minFPos;
                    CalcF(surrentPoss[i], endPos);
                    openList.Add(surrentPoss[i]);
                }
            }

            if (openList.IndexOf(endPos) > -1)
            {
                break;
            }
        }

        List<AStarNode> surroundPoints = GetSurroundPoints(startPos);


        return Temp;
    }

    /// <summary>
    /// 得到目前位置的周围点
    /// </summary>
    /// <returns></returns>
    public List<AStarNode> GetSurroundPoints(AStarNode pointPos)
    {
        List<AStarNode> tempLsit = new List<AStarNode>();

        AStarNode up = null, down = null, left = null, right = null;
        AStarNode lu = null, ru = null, ld = null, rd = null; //左上leftup == lu

        #region point
        if (pointPos.Y < height - 1)
        {
            up = map[pointPos.X, pointPos.Y + 1];
        }

        if (pointPos.Y > 0)
        {
            down = map[pointPos.X, pointPos.Y - 1];
        }

        if (pointPos.X > 0)
        {
            left = map[pointPos.X - 1, pointPos.Y];
        }

        if (pointPos.X < width - 1)
        {
            right = map[pointPos.X + 1, pointPos.Y];
        }

        if (left != null && up != null)
        {
            lu = map[pointPos.X - 1, pointPos.Y + 1];
        }

        if (left != null && down != null)
        {
            ld = map[pointPos.X - 1, pointPos.Y - 1];
        }

        if (right != null && up != null)
        {
            ru = map[pointPos.X + 1, pointPos.Y + 1];
        }

        if (right != null && down != null)
        {
            rd = map[pointPos.X + 1, pointPos.Y - 1];
        }
        #endregion

        #region addList
        if (up != null && !up.NoPass)
        {
            tempLsit.Add(up);
        }

        if (down != null && !down.NoPass)
        {
            tempLsit.Add(down);
        }

        if (left != null && !left.NoPass)
        {
            tempLsit.Add(left);
        }

        if (right != null && !right.NoPass)
        {
            tempLsit.Add(right);
        }

        if (lu != null && !lu.NoPass && !left.NoPass && !up.NoPass)
        {
            tempLsit.Add(lu);
        }

        if (ld != null && !ld.NoPass && !left.NoPass && !down.NoPass)
        {
            tempLsit.Add(ld);
        }

        if (ru != null && !ru.NoPass)
        {
            tempLsit.Add(ru);
        }

        if (rd != null && !rd.NoPass)
        {
            tempLsit.Add(rd);
        }
        #endregion

        return tempLsit;
    }
    /// <summary>
    /// 计算F值  
    /// F = G + H
    /// G 表示从起点 A 移动到网格上指定方格的移动耗费(可沿斜方向移动).
    /// H 表示从指定的方格移动到终点 B 的预计耗费(H 有很多计算方法, 这里我们设定只可以上下左右移动).
    /// </summary>
    private void CalcF(AStarNode nowPos, AStarNode endPos)
    {
        float h = Mathf.Abs(endPos.X - nowPos.X) + Mathf.Abs(endPos.Y - nowPos.Y);
        float g = 0;

        if (nowPos.Parent != null)
        {
            g = CalcG(nowPos, nowPos.Parent);
        }
        float f = g + h;

        nowPos.F = f;
        nowPos.G = g;
        nowPos.H = h;
    }
    /// <summary>
    /// 计算G值  
    /// </summary>
    private float CalcG(AStarNode nowPos, AStarNode parent)
    {
        return Vector2.Distance(new Vector2(nowPos.X, nowPos.Y),
            new Vector2(parent.X, parent.Y)) + parent.G;
    }
    /// <summary>
    /// 寻找openlist中的最小F位置
    /// </summary>
    /// <param name="openList"></param>
    private AStarNode FindMinFPos(List<AStarNode> openList)
    {
        AStarNode temp = null;
        float f = float.MaxValue;
        for (int i = 0; i < openList.Count; i++)
        {
            if (openList[i].F < f)
            {
                f = openList[i].F;
                temp = openList[i];
            }
        }
        return temp;
    }

    /// <summary>
    /// 剔除，把已经存在close surrentPos剔除
    /// </summary>
    /// <param name="surrentPoints"></param>
    /// <param name="closeList"></param>
    public void Filter(List<AStarNode> surrentPos, List<AStarNode> closeList)
    {
        for (int i = 0; i < closeList.Count; i++)
        {
            if (surrentPos.IndexOf(closeList[i]) > -1)
            {
                surrentPos.Remove(closeList[i]);
            }
        }
    }
    /// <summary>
    /// 倒序显示
    /// </summary>
    /// <param name="endPos"></param>
    public void ShowPath(AStarNode endPos)
    {
        AStarNode temp = endPos;
        while (true)
        {
            Debug.Log(temp.X + "--" + temp.Y);
            if (temp.Parent == null)
            {
                break;
            }
            temp = temp.Parent;
        }
    }
}

/****************************************************
    文件：TestAStar.cs
	功能：测试
*****************************************************/

using UnityEngine;

public class TestAStar : MonoBehaviour
{
    private void Start()
    {
        AStarMgr.Instance.InitMap(15, 15);

        AStarNode starPos = AStarMgr.Instance.map[2, 3];
        AStarNode endPos = AStarMgr.Instance.map[6, 3];

        AStarMgr.Instance.FindPath(starPos, endPos);
        AStarMgr.Instance.ShowPath(endPos);

    }
}