A*算法

看完视频后理解的A*算法
A *算法

Node 定义小方格

public class Node
{
public bool walkable;
public int x;
public int z;
public Vector3 pos; // 当前位置
public int hCost; // 从当前节点到目标节点的距离
public int gCost; // 从起始点到当前节点的距离
public Node parent;

public int fCost { get { return gCost + hCost; } }     //   最终的距离

public Node(int x,int z,bool walkable,Vector3 pos)
{
    this.x = x;
    this.z = z;
    this.walkable = walkable;
    this.pos = pos;
}

}

Grids 绘制地图

public class Grids : MonoBehaviour
{
public LayerMask unwalkbaleMask; // 定义障碍物
public Vector3 gridSize; // 当前网格尺寸
public float nodeRadius; // 节点的半径
float nodeDiameter; // 节点的直径
public int nodeNumX,nodeNumZ; // 定义每个节点的数量，分别是x和y的数量
public Node[,] grid; // 定义的节点，利用二维节点数组来定义网格
public List path; // 定义寻路路径列表，记录寻路的经过的节点
public Transform Player, Target;
Node playerNode, targetNode;

// Start is called before the first frame update
void Start()
{
    nodeDiameter = nodeRadius * 2;
    nodeNumX = Mathf.RoundToInt(gridSize.x / nodeDiameter);         // Mathf.RoundToInt()   整形化
    nodeNumZ = Mathf.RoundToInt(gridSize.z / nodeDiameter);
    CreateGrid();
}

public void CreateGrid()
{
    grid = new Node[nodeNumX,nodeNumZ];
    Vector3 startPos = transform.position - new Vector3(gridSize.x / 2, 0, gridSize.z / 2);         //地图左下角的位置坐标
    for (int x = 0; x < nodeNumX; x++)
    {
        for (int z = 0; z < nodeNumZ; z++)
        {
            Vector3 currentPos = startPos + new Vector3(x * nodeDiameter + nodeRadius, 0, z * nodeDiameter + nodeRadius);
            //  设置每个节点（小方格）的中心位置坐标
            bool walkable = !Physics.CheckSphere(currentPos, nodeRadius, unwalkbaleMask);
            //  在每个小方块位置处检查一个小圆球范围内是否有物体
            //  并且这个物体的层级是unwalkbaleMask  
            //  如果是将这个节点定义为不可移动点  返回false 如果不是返回true
            grid[x,z]=new Node(x,z,walkable,currentPos);
            //  给每个节点小方格设置数据
        }
    }
}
public Node GetNodeFromPosition(Vector3 pos)            //  该方法从某个位置获取节点
{
    float penrcentX = (pos.x + gridSize.x / 2) / gridSize.x;
    penrcentX = Mathf.Clamp01(penrcentX);                       //  使penrcentX范围在0-1之间
    float penrcentZ = (pos.z + gridSize.z / 2) / gridSize.z;
    penrcentZ = Mathf.Clamp01(penrcentZ);                       //  使penrcentX范围在0-1之间
    int x = Mathf.RoundToInt(penrcentX * (nodeNumX - 1));
    int z = Mathf.RoundToInt(penrcentZ * (nodeNumZ - 1));
    return grid[x, z];
}

private void OnDrawGizmos()         //画出方格
{
    Gizmos.color = Color.green;
    Gizmos.DrawCube(transform.position, new Vector3(gridSize.x,1, gridSize.z ));
    if (grid!=null)
    {
        foreach (Node n in grid)
        {
            if (n.walkable)
            {
                Gizmos.color = Color.gray;
                if (GetNodeFromPosition(Player.position)==n)
                {
                    Gizmos.color = Color.red;
                }
                if (GetNodeFromPosition(Target.position) == n)
                {
                    Gizmos.color = Color.white;
                }
                if (path != null && path.Contains(n))
                {
                    Gizmos.color = Color.blue;
                }
                    
                Gizmos.DrawCube(n.pos,new Vector3(nodeDiameter*0.9f,1.2f,nodeDiameter*0.9f));
            }
        }
    }
}

}

FindPath 使用A*算法寻路

public class PathFinding : MonoBehaviour
{
Grids myGrid;
Node startNode;
Node targetNode;
// Start is called before the first frame update
void Awake()
{
myGrid = GetComponent();
}

void FindPath(Vector3 startPos,Vector3 targetPos)
{
    startNode = myGrid.GetNodeFromPosition(startPos);       //  得到开始节点
    targetNode = myGrid.GetNodeFromPosition(targetPos);     //  得到结束节点
    List<Node> openSet = new List<Node>();                  //  开始节点列表
    HashSet<Node> closeSet = new HashSet<Node>();           //  关闭节点列表
    openSet.Add(startNode);                                 //  开始节点加入到openSet
    while (openSet.Count>0)                                 //  openSet中有节点开始循环
    {
        Node currentNode = openSet[0];                      //  当前节点
        for (int i = 0; i < openSet.Count; i++)             //  开始循环列表    
        {
            if (openSet[i].fCost<currentNode.fCost || (openSet[i].fCost==currentNode.fCost && openSet[i].hCost<currentNode.hCost))
            {
                currentNode = openSet[i];                   //  满足条件重新得到当前节点是openSet[i]，
            }
            openSet.Remove(currentNode);                    //  在openSet中移除当前节点
            closeSet.Add(currentNode);                      //  在closeSet中加入当前节点
            if (currentNode==targetNode)                    //  到指定位置
            {
                print("path was found");
                RetrievePath(currentNode);                  //  调用 RetrievePath()方法
                return;
            }
            //print("Path");
            foreach (Node n in GetNeighbors(currentNode))           //  遍历周围一圈节点即遍历 neighbor 列表 调用GetNeighbors
            {                                                       //  GetNeighbors(currentNode)可以得到当前节点周围的邻居节点
                if (!n.walkable || closeSet.Contains(n))            //  如果该节点为不能走的节点或者在关闭列表中有该节点
                    continue;                                       //  结束本次循环，进行下一个节点的判断
                int newgCost = currentNode.gCost + GetDistance(currentNode, n);     //  当前节点和邻居节点之间的距离
                bool inOpenset = openSet.Contains(n);               //  如果打开列表中有该节点返回true
                if (newgCost<n.gCost || !inOpenset)                 //  如果newgCost小于邻居节点从起始点到当前节点的距离
                {                                                   //  或者该邻居节点不在打开节点列表中
                    n.gCost = newgCost;                             //  此时节点n的 gCost为newgCost
                    n.hCost = GetDistance(n, targetNode);           //  求出此时节点n到终点的距离
                    n.parent = currentNode;                         //  设置父节点
                    if (!inOpenset)                                 //  如果不在打开节点列表中
                    {
                       // Debug.Log("aa");
                        openSet.Add(n);                             //  把节点n添加到打开列表中
                    }
                }
                
            }
        }
    }
}

private void RetrievePath(Node n)           //  上方调用该函数时传递的参数为当前节点
{
    List<Node> p = new List<Node>();        //  新建列表 p,p是用来存储寻路的路径
    while (n!=startNode)                    //  如果当前节点不是开始位置节点
    {
        p.Add(n);                           //  列表p中添加节点n,n为当前节点
        n = n.parent;                       //  n重新赋值，赋值为它的父节点，直到n为开始位置节点
    }
    p.Reverse();                            //  因为赋值节点是从后往前，所以要反转顺序
    myGrid.path = p;                        //  把路径返回
}

int GetDistance(Node n1,Node n2)              //  求距离(pos1和pos2之间的距离)
{
    int distanceX = (int)Mathf.Abs(n1.x - n2.x);    
    int distanceZ = (int)Mathf.Abs(n1.z - n2.z);
    if (distanceX > distanceZ)                      //  计算距离的方式，水平垂直方向一格为10，斜方向为14  
    {
        return 14 * distanceZ + 10 * (distanceX - distanceZ);
    }
    else
    {
        return 14 * distanceX + 10 * (distanceZ - distanceX);
    }
}

private List<Node> GetNeighbors(Node n)                 //  获得周围一圈节点，上方调用该函数传递的参数为当前节点位置 
{                                                       //  n是当前位置传递进来的坐标
    List<Node> neighbors = new List<Node>();            //  新建列表 neighbor 用来存放当前节点周围一圈的节点
    int xx = n.x;                                       //  xx是当前位置的x轴上的值
    int zz = n.z;                                       //  zz是当前位置的z轴上的值
    Debug.Log(xx + "," + zz);
    Debug.Log("a="+n.pos);
    for (int x = -1; x <= 1; x++)                       //  获取周围一圈节点坐标的x
    {
        for (int z = -1; z <= 1; z++)                   //  获取周围一圈节点坐标的z
        {
            if (x == 0 && z == 0)                       //  如果x=0，z=0即位置没发生改变，得到的是自己当前坐标，不需要
                continue;
            if (xx+x >= 0 && xx+x < myGrid.nodeNumX && zz + z >= 0 && zz + z < myGrid.nodeNumZ)     //  获取周围一圈节点坐标
            {
                neighbors.Add(myGrid.grid[xx + x, zz + z]);     //  把每个邻居节点的信息存储到 neighbor 列表中
            }                                                   //  在Grids中从左下角第一个方格开始[x,z]是[0,0]
        }
    }
    return neighbors;               //  返回 neighbor 列表
}

// Update is called once per frame
void Update()
{
    FindPath(myGrid.Player.position, myGrid.Target.position);
}

}