C#数据结构-A星寻路算法

A*算法其实也不复杂，首先有以下几个概念：

• 开启的节点表(OpenList)

  存放着所有的待检测的节点（坐标），每次都会从其中寻找出符合某个条件的坐标。

• 关闭的节点表(ClosedList)

  存放着所有不会被检测的节点（坐标），每次检测都会忽略它们。

首先，我们定义了两个点，分别是起点和终点。

整个算法的核心就是启发式的权值比较，分为G和H值。

• G值

  是从起点到某一点积累的移动值。一般我们将从起点按非斜向方向移动的G值定为10，斜向为14，走一步必须将此节点的值增加，也就是说移动到的节点的G值等于移动前节点的G值加上按方向走到该店的G值的增加量。

  打个比方，移动前(0,0)的G值为0，则(0,1)的G值就是10，(1,2)的G值就是24

• H值

  该值和终点坐标相关，一般常用的曼哈顿算法为: abs(dx-x)+abs(dy-y)，也就是横坐标的差值加上纵坐标的差值。

• F值

  F值为G值和H值之和

上述的基本概念理清的话，下面的算法就简单了。

算法的基本逻辑基本按下述步骤走：

1.将起点放入OpenList中

2.将OpenList中最小F值的节点(MinFNode)取出，从OpenList移除，并放入ClosedList中

3.遍历MinFNode周围的节点，忽略障碍节点和已在ClosedList中的节点，这里会有3种情况

• 相邻点不在OpenList中的，简单的计算好H值和G值（MinFNode的G值加上移动所产生的G值），并且把该相邻点的父节点设置为MinFNode (后期找到终点后，需要用父节点进行路径回溯)
• 相邻点已在OpenList中的，则判断从MinFNode节点的G值加上到相邻点移动所产生的G值之和，是否小于该相邻点的G值，假设小于了，则更新该相邻点的G值为较小的那个，然后重新设置该相邻点的父节点为MinFNode
• 假设遍历到的节点是终点，则按MinFNode的父节点进行回溯，获取到起点的路径，找到最终路径

4.如果没有找到终点，回到第二步，继续执行

注：下面的代码在Unity里用C#实现，整个工程我放在Github上了，获取地址超链接

Node节点实现：

using UnityEngine;

public class Node
{
    //是否可以通过
    public bool m_CanWalk;
    //节点空间位置
    public Vector3 m_WorldPos;
    //节点在数组的位置
    public int m_GridX;
    public int m_GridY;
    //开始节点到当前节点的距离估值
    public int m_gCost;
    //当前节点到目标节点的距离估值
    public int m_hCost;

    public int FCost
    {
        get { return m_gCost + m_hCost; }
    }
    //当前节点的父节点
    public Node m_Parent;

    public Node(bool canWalk, Vector3 position, int gridX, int gridY)
    {
        m_CanWalk = canWalk;
        m_WorldPos = position;
        m_GridX = gridX;
        m_GridY = gridY;
    }
}

创建网格：

using System.Collections.Generic;
using UnityEngine;

public class GridBase : MonoBehaviour
{
    private Node[,] m_Grid;
    public Vector2 m_GridSize;
    public float m_NodeRadius;
    public LayerMask m_Layer;
    public Stack<Node> m_Path = new Stack<Node>();
    private float m_NodeDiameter;
    private int m_GridCountX;
    private int m_GridCountY;

    void Start()
    {
        m_NodeDiameter = m_NodeRadius * 2;
        m_GridCountX = Mathf.RoundToInt(m_GridSize.x / m_NodeDiameter);
        m_GridCountY = Mathf.RoundToInt(m_GridSize.y / m_NodeDiameter);
        m_Grid = new Node[m_GridCountX, m_GridCountY];
        CreateGrid();
    }

    /// <summary>
    /// 创建格子
    /// </summary>
    private void CreateGrid()
    {
        Vector3 startPos = transform.position;
        startPos.x = startPos.x - m_GridSize.x / 2;
        startPos.z = startPos.z - m_GridSize.y / 2;
        for (int i = 0; i < m_GridCountX; i++)
        {
            for (int j = 0; j < m_GridCountY; j++)
            {
                Vector3 worldPos = startPos;
                worldPos.x = worldPos.x + i * m_NodeDiameter + m_NodeRadius;
                worldPos.z = worldPos.z + j * m_NodeDiameter + m_NodeRadius;
                bool canWalk = !Physics.CheckSphere(worldPos, m_NodeRadius, m_Layer);
                m_Grid[i, j] = new Node(canWalk, worldPos, i, j);
            }
        }
    }

    /// <summary>
    /// 通过空间位置获得对应的节点
    /// </summary>
    /// <param name="pos"></param>
    /// <returns></returns>
    public Node GetFromPosition(Vector3 pos)
    {
        float percentX = (pos.x + m_GridSize.x / 2) / m_GridSize.x;
        float percentZ = (pos.z + m_GridSize.y / 2) / m_GridSize.y;
        percentX = Mathf.Clamp01(percentX);
        percentZ = Mathf.Clamp01(percentZ);
        int x = Mathf.RoundToInt((m_GridCountX - 1) * percentX);
        int z = Mathf.RoundToInt((m_GridCountY - 1) * percentZ);
        return m_Grid[x, z];
    }

    /// <summary>
    /// 获得当前节点的相邻节点
    /// </summary>
    /// <param name="node"></param>
    /// <returns></returns>
    public List<Node> GetNeighor(Node node)
    {
        List<Node> neighborList = new List<Node>();
        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                if (i == 0 && j == 0)
                {
                    continue;
                }
                int tempX = node.m_GridX + i;
                int tempY = node.m_GridY + j;
                if (tempX < m_GridCountX && tempX > 0 && tempY > 0 && tempY < m_GridCountY)
                {
                    neighborList.Add(m_Grid[tempX, tempY]);
                }
            }
        }
        return neighborList;
    }

    private void OnDrawGizmos()
    {
        Gizmos.DrawWireCube(transform.position, new Vector3(m_GridSize.x, 1, m_GridSize.y));
        if (m_Grid == null)
        {
            return;
        }
        foreach (var node in m_Grid)
        {
            Gizmos.color = node.m_CanWalk ? Color.white : Color.red;
            Gizmos.DrawCube(node.m_WorldPos, Vector3.one * (m_NodeDiameter - 0.1f));
        }
        if (m_Path != null)
        {
            foreach (var node in m_Path)
            {
                Gizmos.color = Color.green;
                Gizmos.DrawCube(node.m_WorldPos, Vector3.one * (m_NodeDiameter - 0.1f));
            }
        }
    }
}

寻路算法的实现：

using System.Collections.Generic;
using UnityEngine;

public class FindPath : MonoBehaviour
{
    public Transform m_StartNode;
    public Transform m_EndNode;
    private GridBase m_Grid;
    private List<Node> openList = new List<Node>();
    private HashSet<Node> closeSet = new HashSet<Node>();

    void Start()
    {
        m_Grid = GetComponent<GridBase>();
    }

    void Update()
    {
        FindingPath(m_StartNode.position, m_EndNode.position);
    }

    /// <summary>
    /// A*算法，寻找最短路径
    /// </summary>
    /// <param name="start"></param>
    /// <param name="end"></param>
    private void FindingPath(Vector3 start, Vector3 end)
    {
        Node startNode = m_Grid.GetFromPosition(start);
        Node endNode = m_Grid.GetFromPosition(end);
        openList.Clear();
        closeSet.Clear();
        openList.Add(startNode);
        int count = openList.Count;
        while (count > 0)
        {
            // 寻找开启列表中的F最小的节点，如果F相同，选取H最小的
            Node currentNode = openList[0];
            for (int i = 0; i < count; i++)
            {
                Node node = openList[i];
                if (node.FCost < currentNode.FCost || node.FCost == currentNode.FCost && node.m_hCost < currentNode.m_hCost)
                {
                    currentNode = node;
                }
            }
            // 把当前节点从开启列表中移除，并加入到关闭列表中
            openList.Remove(currentNode);
            closeSet.Add(currentNode);
            // 如果是目的节点，返回
            if (currentNode == endNode)
            {
                GeneratePath(startNode, endNode);
                return;
            }
            // 搜索当前节点的所有相邻节点
            foreach (var node in m_Grid.GetNeighor(currentNode))
            {
                // 如果节点不可通过或者已在关闭列表中，跳出
                if (!node.m_CanWalk || closeSet.Contains(node))
                {
                    continue;
                }
                int gCost = currentNode.m_gCost + GetDistanceNodes(currentNode, node);
                // 如果新路径到相邻点的距离更短 或者不在开启列表中
                if (gCost < node.m_gCost || !openList.Contains(node))
                {
                    // 更新相邻点的F，G，H
                    node.m_gCost = gCost;
                    node.m_hCost = GetDistanceNodes(node, endNode);
                    // 设置相邻点的父节点为当前节点
                    node.m_Parent = currentNode;
                    // 如果不在开启列表中，加入到开启列表中
                    if (!openList.Contains(node))
                    {
                        openList.Add(node);
                    }
                }
            }
        }
    }

    /// <summary>
    /// 生成路径
    /// </summary>
    /// <param name="startNode"></param>
    /// <param name="endNode"></param>
    private void GeneratePath(Node startNode, Node endNode)
    {
        Stack<Node> path = new Stack<Node>();
        Node node = endNode;
        while (node.m_Parent != startNode)
        {
            path.Push(node);
            node = node.m_Parent;
        }
        m_Grid.m_Path = path;
    }

    /// <summary>
    /// 获得两个节点的距离
    /// </summary>
    /// <param name="node1"></param>
    /// <param name="node2"></param>
    /// <returns></returns>
    private int GetDistanceNodes(Node node1, Node node2)
    {
        int deltaX = Mathf.Abs(node1.m_GridX - node2.m_GridX);
        int deltaY = Mathf.Abs(node1.m_GridY - node2.m_GridY);
        if (deltaX > deltaY)
        {
            return deltaY * 14 + 10 * (deltaX - deltaY);
        }
        else
        {
            return deltaX * 14 + 10 * (deltaY - deltaX);
        }
    }
}