简易的A*算法自动寻路

参考：

曼哈顿距离，横向和纵向直线距离，仅限于横向纵向移动
对角线距离，对角线 + 直线，可以横向、纵向、对角线方向移动
欧几里得距离，任意角度直线，任意方向移动

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

public class AStartTest
{

    AStarNode[,] nodeMap = new AStarNode[10, 10];

    void CheckAndFindPath(AStarNode startNode, AStarNode endNode)
    {
        //计算路径
        List<AStarNode> pathNodes = FindNodePath(startNode, endNode, nodeMap);

        if (pathNodes == null || pathNodes.Count == 0)
            return;

        //计算路径折点
        List<AStarNode> pathPoints = GetPathPoint(pathNodes);
    }

    //计算路径折点
    List<AStarNode> GetPathPoint(List<AStarNode> path)
    {
        List<AStarNode> tmpPointList = new List<AStarNode>();

        //无折点
        if (path.Count <= 2)
            return tmpPointList;
            
        //当前节点与前一节点的位置关系(X坐标相同或Y坐标相同)
        bool lastDirIsX = path[1].pos.x == path[0].pos.x;

        //计算折点
        for (int i = 2; i < path.Count; i++)
        {
            //若与前一节点X坐标相同
            if (path[i].pos.x == path[i - 1].pos.x)
            {
                //前两个节点时Y坐标相同，即为折点
                if (!lastDirIsX)
                {
                    //记录折点，记录当前节点关系
                    tmpPointList.Add(path[i - 1]);
                    lastDirIsX = true;
                }
            }
            else
            {
                if (lastDirIsX)
                {
                    tmpPointList.Add(path[i - 1]);
                    lastDirIsX = false;
                }
            }
        }
        //路径最后节点也视为折点
        tmpPointList.Add(path[path.Count - 1]);
        //
        return tmpPointList;
    }

    #region --- A*算法 ---

    //计算最短有效路径
    List<AStarNode> openList = new List<AStarNode>();
    List<AStarNode> closeList = new List<AStarNode>();
    List<AStarNode> aroundNodes;
    List<AStarNode> FindNodePath(AStarNode startNode, AStarNode endNode, AStarNode[,] allNodes)
    {
        //计算范围内的节点
        openList.Clear();
        //不在计算范围内的节点
        closeList.Clear();

        //添加起点
        openList.Add(startNode);

        AStarNode curNode;
        //从起点开始循环判断
        while (openList.Count > 0)
        {
            //初始当前位置
            curNode = openList[0];
            //计算最优当前位置
            for (int i = 0; i < openList.Count; i++)
            {
                //F:从起点到目标点的移动步数
                //H:从当前位置到目标位置的移动步数
                if (openList[i].CostF < curNode.CostF && openList[i].costH < curNode.costH)
                {
                    curNode = openList[i];
                }
            }
            //锁定当前位置节点
            openList.Remove(curNode);
            closeList.Add(curNode);

            ////已经计算到目标点
            //if (curNode.Equals(endNode))
            //{
            //    //返回最优路径
            //    return GetPathWithNode(startNode, endNode);
            //}

            //未计算到目标点, 继续

            //获取当前点的周围节点, 在周围节点中查找下一步的最优节点
            aroundNodes = GetAroundNodes(curNode, allNodes);
            for (int i = 0; i < aroundNodes.Count; i++)
            {
                //计算到目标点
                if (aroundNodes[i].Equals(endNode))
                {
                    //设置上一节点
                    aroundNodes[i].lastNode = curNode;
                    //返回最优路径
                    return GetPathWithNode(startNode, endNode);
                }

                //不是目标点, 继续计算, 剔除周围节点不可通过、在不可计算范围内的节点
                if (!aroundNodes[i].isWall && !closeList.Contains(aroundNodes[i]))
                {
                    //计算 G H F
                    //F:从起点到目标点的移动步数

                    //G:从起点到当前位置的移动步数
                    int newCostG = curNode.costG + GetNodesDistance(curNode, aroundNodes[i]);

                    if (newCostG <= aroundNodes[i].costG || !openList.Contains(aroundNodes[i]))
                    {
                        //刷新赋值
                        aroundNodes[i].costG = newCostG;
                        //H:从当前位置到目标位置的移动步数
                        aroundNodes[i].costH = GetNodesDistance(aroundNodes[i], endNode);
                        //设置上级节点
                        aroundNodes[i].lastNode = curNode;
                        //添加到计算范围内
                        if (!openList.Contains(aroundNodes[i]))
                        {
                            openList.Add(aroundNodes[i]);
                        }
                    }
                }
            }
        }
        return null;
    }

    //计算距离
    int GetNodesDistance(AStarNode startNode, AStarNode endNode)
    {
        return Mathf.Abs(startNode.pos.x - endNode.pos.x) + Mathf.Abs(startNode.pos.y - endNode.pos.y);
    }

    //周围节点只取上下左右四个, 不取对角线（根据实际需求设置周围节点）
    Vector2Int[] aroundPos = { new Vector2Int(0, 1), new Vector2Int(0, -1), new Vector2Int(1, 0), new Vector2Int(-1, 0) };
    
    //获取周围Node
    List<AStarNode> tmpAroundList = new List<AStarNode>();
    List<AStarNode> GetAroundNodes(AStarNode curNode, AStarNode[,] allNodes)
    {
        tmpAroundList.Clear();
        for (int i = 0; i < aroundPos.Length; i++)
        {
            //计算周围节点坐标
            int x = curNode.pos.x + aroundPos[i].x;
            int y = curNode.pos.y + aroundPos[i].y;

            //剔除不在取值范围内的数据
            if (x >= 0 && x < allNodes.GetLength(0) && y >= 0 && y < allNodes.GetLength(1))
            {
                if (allNodes[x, y] != null)
                    tmpAroundList.Add(allNodes[x, y]);
            }
        }
        return tmpAroundList;
    }

    //获取路径(包含起点)
    List<AStarNode> tmpNodePath = new List<AStarNode>();
    List<AStarNode> GetPathWithNode(AStarNode startNode, AStarNode endNode)
    {
        tmpNodePath.Clear();
        if (endNode != null)
        {
            //逆向查找路径
            AStarNode temp = endNode;
            while (temp != startNode)
            {
                tmpNodePath.Add(temp);
                temp = temp.lastNode;
            }
            tmpNodePath.Add(startNode);
            //路径数据反向
            tmpNodePath.Reverse();
        }
        return tmpNodePath;
    }

#endregion

}

// A*数据节点类

public class AStarNode
{
    //A*算法节点类

    //是否能通过
    public bool isWall;
    //位置坐标
    public Vector2Int pos;

    //上个节点
    public AStarNode lastNode;

    //从起点到当前位置的移动步数
    public int costG;
    //从当前位置到目标位置的移动步数
    public int costH;
    //从起点到目标点的移动步数
    public int CostF
    {
        get { return costG + costH; }
    }

    public AStarNode(bool _isWall, Vector2Int _pos)
    {
        isWall = _isWall;
        pos = _pos;
    }

    //重写Equals
    public override bool Equals(object obj)
    {
        if (obj is AStarNode)
        {
            AStarNode objNode = (AStarNode)obj;
            return objNode.pos == pos;
        }
        return false;
    }
    public override int GetHashCode()
    {
        return base.GetHashCode();
    }
}

简易的A*算法 自动寻路

猜你喜欢

简易的A*算法自动寻路