unity A星寻路

算法 fCost = gCost + hCost
gCost 是当前节点到移动起始点的消耗，hCost是当前节点到终点的消耗
网格为变成为1的矩形，左右相邻的两个网格直接的gCost为1，斜对角相邻的两个网格的gCost为1.4
hCost 当前网格到终点网格的 水平距离 + 垂直距离
比如当前网格位置是（2，3），终点位置（10，8），则hCost = (10-2) + (8-3)
原始的算法是 fCost = gCost + hCost，均匀计算周围网格
对于一些比较复杂的散装障碍物场景，我们可以适当调整gCost和hCost的权重，比如 fCost = gCost x 4 + hCost x 6，这个修改会让寻路的时候，在消耗相同的网格点上，优先在距离终点位置更近的网格周围寻路。当我们从（1，1）向（3，10）位置寻路的时候，我们在店（3，1）和点（1，10）两个位置的 gCost + hCost 是相等的，但是点（1，10）其实距离终点更近一些，用调整过权重的算法，（1，10）我们得出来的的值会小一些，就可以优先从（1，10）位置寻找可移动路径

代码如下
寻路节点

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

public class AstarNode
{
     public int gridX;
     public int gridY;
     public Vector3 position;

     public float hCost;
     public float gCost;
     public float fCost {
          get
          {
               return gCost + hCost;
          }
     }
     
     /// <summary>
     /// 优先处理距离终点近的node
     /// </summary>
     public float fCostNew {
          get
          {
               return gCost*4 + hCost*6;
          }
     }
     public bool walkable;
     public AstarNode parent;

     public AstarNode()
     {
          
     }
     public AstarNode(int x, int y, Vector3 pos, bool canWalk)
     {
          gridX = x;
          gridY = y;
          position = pos;
          walkable = canWalk;
     }
     
     
 
}

地图网格生成

using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using UnityEngine;
using Random = System.Random;
public enum SearchType
{
    NeighborFour,
    NeighborEight,
}
public class MapGridsManager : MonoBehaviour
{
    public int width = 100;
    public int height = 100;
    public float cellSize = 1f;
    public Mesh mesh;
    private Vector3 originPoint;
    private AstarNode[,] grids;
    public AStartManager manager;
    public AstartMoveTest moveTarget;
    public bool isUseCache = false;
    public bool isUseNewCost = false;
    public SearchType searchType = SearchType.NeighborFour;
    [Header("障碍物生成范围") ]
    public Rect obstacleRect;
    void Awake()
    {
        originPoint = Vector3.zero;
        mesh = Resources.GetBuiltinResource<Mesh>("Quad.fbx");
    }

    private void Start()
    {
        grids = new AstarNode[width, height];
        Dictionary<int, AstarNode> nodeDic = GetCacheData();
        for (int i = 0; i < width; i++)
        {
            for (int j = 0; j < height; j++)
            {
                int keyId = i * 100000 + j;
               
                AstarNode node;
                if (isUseCache && nodeDic.ContainsKey(keyId))
                {
                    node = nodeDic[keyId];
                }
                else
                {
                    bool isObstacal = UnityEngine.Random.Range(1, 100) > 50;
                    isObstacal = i >= obstacleRect.xMin && i < obstacleRect.xMax && j >= obstacleRect.yMin && j < obstacleRect.yMax  && isObstacal;
                    bool warkAble = !isObstacal;
                    node = new AstarNode(i, j, originPoint + new Vector3(i * cellSize, 0, j * cellSize), warkAble);    
                }
                
                grids[i, j] = node;
            }
        }

        serilizeNode();
    }

    void serilizeNode()
    {
        StringBuilder sb = new StringBuilder();
        for (int i = 0; i < width; i++)
        {
            for (int j = 0; j < height; j++)
            {
                var node = grids[i, j];
                int warkState = node.walkable ?1:0;
                if (i>0 || j > 0)
                {
                    sb.Append("|");
                }

                var data = $"{node.gridX}_{node.gridY}_{warkState}_{node.position.x}_{node.position.y}_{node.position.z}";
                sb.Append(data) ;
            }
        }

        string res = sb.ToString();
        UnityEngine.PlayerPrefs.SetString("grid_data", res);
    }

    Dictionary<int, AstarNode> GetCacheData()
    {
        Dictionary<int, AstarNode> nodeDic = new Dictionary<int, AstarNode>(width * height);
        string data = UnityEngine.PlayerPrefs.GetString("grid_data", "");
        string[] arr = data.Split("|");
        for (int i = 0; i < arr.Length; i++)
        {
            string itemStr = arr[i];
            string[] itemArr = itemStr.Split("_");
            if (itemArr.Length < 6)
            {
                continue;
            }
            AstarNode node = new AstarNode();
            node.gridX = int.Parse(itemArr[0]);
            node.gridY = int.Parse(itemArr[1]);
            node.walkable = int.Parse(itemArr[2]) == 1 ? true : false;
            float positionX = float.Parse(itemArr[3]);
            float positionY = float.Parse(itemArr[4]);
            float positionZ = float.Parse(itemArr[5]);
            node.position = new Vector3(positionX, positionY, positionZ);
            int keyId = node.gridX * 100000 + node.gridY;
            nodeDic.Add(keyId, node);
        }
        return nodeDic;
    }
    
    public AstarNode GetNode(int x, int y)
    {
        if (x < 0 || x >= width || y < 0 || y >= height)
        {
            return null;
        }
        return grids[x, y];
    }
    
    

    public List<AstarNode> GetNeighborNodes(AstarNode node)
    {
        if (searchType == SearchType.NeighborFour)
            return GetNeighborFourNodes(node);
        else
        {
            return GetNeighborEightNodes(node);
        }
    }

    public List<AstarNode> GetNeighborFourNodes(AstarNode node)
    {
        List<AstarNode> nodeList = new List<AstarNode>();
        int nodeX = node.gridX;
        int nodeY = node.gridY;
        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                if (i == 0 && j == 0)
                {
                    continue;
                }

                if (i == 0 || j == 0)
                {
                    int itemX = nodeX + i;
                    int itemY = nodeY + j;
                    AstarNode itemNode = GetNode(itemX, itemY);
                    if (itemNode != null)
                    {
                        nodeList.Add(itemNode);
                    }
                }
            }
        }

        return nodeList;
    }
    
    public List<AstarNode> GetNeighborEightNodes(AstarNode node)
    {
        List<AstarNode> nodeList = new List<AstarNode>();
        int nodeX = node.gridX;
        int nodeY = node.gridY;
        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                if (i == 0 && j == 0)
                {
                    continue;
                }
                int itemX = nodeX + i;
                int itemY = nodeY + j;
                AstarNode itemNode = GetNode(itemX, itemY);
                if (itemNode != null)
                {
                    nodeList.Add(itemNode);
                }
            }
        }

        return nodeList;
    }








    public AstarNode GetByWorldPositionNode(Vector3 pos)
    {
        int x = Mathf.FloorToInt(pos.x / cellSize);
        int y = Mathf.FloorToInt(pos.y / cellSize);
        if (x < 0 || y < 0 || x>=width || y >= height)
        {
            return null;
        }
        return grids[x, y];
    }

    void OnDrawGizmos()
    {
        Gizmos.color = Color.gray;
        if (grids != null)
        {
            for (int i = 0; i < width; i++)
            {
                for (int j = 0; j < height; j++)
                {
                    AstarNode node = grids[i, j];
                    
                    if (manager != null)
                    {
                        if (manager.rodeList.Contains(node))
                        {
                            Gizmos.color = Color.white;
                        }
                        else if(manager.openList.Contains(node))
                        {
                            Gizmos.color = Color.black;
                        }
                        else if (manager.closeList.Contains(node))
                        {
                            Gizmos.color = Color.green;
                        }
                        else
                        {
                            Gizmos.color = node.walkable?  Color.blue: Color.red;
                        }
                        if (manager.moveEndNode != null && manager.moveEndNode == node)
                        {
                            Gizmos.color = Color.yellow;
                        }
                        
                        if (manager.moveStartNode != null && manager.moveStartNode == node)
                        {
                            Gizmos.color = Color.gray;
                        }
                    }
                    else
                    {
                        Gizmos.color = node.walkable?  Color.blue: Color.red;
                    }
                    
                    
                    // Gizmos.DrawCube(node.position, Vector3.one);
                    var rotation = Quaternion.Euler(new Vector3(90, 0, 0));
                    Gizmos.DrawMesh(mesh, node.position, rotation, Vector3.one);
                }
            }
        }

        Gizmos.color = Color.black;
        var halfCellSize = cellSize / 2;
        for (int i = 0; i <= width; i++)
        {
            Gizmos.DrawLine(new Vector3(i-halfCellSize,0,0), new Vector3(i-halfCellSize, 0 ,height-halfCellSize));
        }
        
        for (int i = 0; i <= height; i++)
        {
            Gizmos.DrawLine(new Vector3(0,0,i-halfCellSize), new Vector3(width-halfCellSize, 0 ,i-halfCellSize));
        }
         
    }

    public void ResetNodeCost()
    {
        for (int i = 0; i < width; i++)
        {
            for (int j = 0; j < height; j++)
            {
                var node = grids[i, j];
                node.gCost = 0;
                node.hCost = 0;
            }
        }
    }

    public void BeginMove(List<AstarNode> rodeNodeList)
    {
        moveTarget.BeginMove(rodeNodeList);
    }
}

寻路

using System;
using System.Collections;
using System.Collections.Generic;
using Unity.VisualScripting;
using UnityEngine;

[Serializable]
public class ObstacleRange
{
    public int x;
}
public class AStartManager : MonoBehaviour
{
    public MapGridsManager map;
    public Vector3 from;
    public Vector3 end;

    public List<AstarNode> openList = new List<AstarNode>();
    public List<AstarNode> closeList = new List<AstarNode>();
    public List<AstarNode> rodeList = new List<AstarNode>();

    public AstarNode moveStartNode;
    public AstarNode moveEndNode;
    private bool isInFindPath = false;//是否正在训练中
    
        
    void Update()
    {
        if (Input.GetKeyUp(KeyCode.A))
        {
            StartCoroutine(FindPath(from, end));
            
        }
        else if(Input.GetMouseButtonDown(0))
        {
            Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
            RaycastHit hit;
            if (Physics.Raycast(ray, out hit))
            {
                var pos = new Vector3(hit.point.x, hit.point.z, 0);
                Debug.Log($"{pos}--{Input.mousePosition}");
                var node = map.GetByWorldPositionNode(pos);
                if (node != null)
                { 
                    from = end;
                    end =  pos;
                    StartCoroutine(FindPath(from, end));
                }
            }
            
        }
        
    }

    void DoMove()
    {
        map.BeginMove(rodeList);
    }

    IEnumerator FindPath(Vector3 startPos, Vector3 endPos)
    {
        if (isInFindPath)
        {
            yield break;
        }

        isInFindPath = true;
        ResetNodeData();
        AstarNode startNode = map.GetByWorldPositionNode(startPos);
        startNode.gCost = 0;
        startNode.hCost = 0;
        startNode.parent = null;
        AstarNode endNode = map.GetByWorldPositionNode(endPos);
        endNode.gCost = 0;
        endNode.hCost = 0;
        endNode.parent = null;
        openList.Add(startNode);
        moveStartNode = startNode;
        moveEndNode = endNode;
        if (!endNode.walkable)
        {
            Debug.LogError("终点不可抵达");
            isInFindPath = false;
            yield break;
        }
        
        int index = 0;
        while (openList.Count > 0)
        {
            var targetNode = openList[0];
            Debug.Log($"{index}: ({targetNode.gridX} ,{targetNode.gridY})");
            openList.Remove(targetNode);
            if (targetNode.gridX == endNode.gridX && targetNode.gridY == endNode.gridY)
            {
                GenerateRote(targetNode);
                openList.Clear();
                break;
            }

            AddOpenList(targetNode, endNode);
            if (map.isUseNewCost)
            {
                openList.Sort((a, b)=>
                {
                    return a.fCostNew.CompareTo(b.fCostNew);
                });
            }
            else
            {
                openList.Sort((a, b)=>
                {
                    return a.fCost.CompareTo(b.fCost);
                });
            }
            
            
            yield return new WaitForSeconds(0.1f);
            index++;
            if (index > 5000)
            {
                Debug.LogError("循环超出上限");
                break;
            }
        }
        Debug.Log("寻路循环次数为：" + index);
        DoMove();
        isInFindPath = false;
    }

    void ResetNodeData()
    {
        openList.Clear();
        closeList.Clear();
        rodeList.Clear();
        map.ResetNodeCost();
    }

    private bool AddOpenList(AstarNode targetNode, AstarNode endNode)
    {
        var neighborNodes = map.GetNeighborNodes(targetNode);
        closeList.Add(targetNode);
        foreach (var item in neighborNodes)
        {
            if (item.walkable == false || closeList.Contains(item) || openList.Contains(item))
            {
                continue;
            }
                    
            float tempGCost = targetNode.gCost + GetDestance(targetNode, item);
            if (item.gCost <= 0 || tempGCost < item.gCost)
            {
                item.parent = targetNode;
                item.gCost = targetNode.gCost + GetDestance(targetNode, item);
                item.hCost = Mathf.Abs(endNode.gridX - item.gridX) + Mathf.Abs(endNode.gridY - item.gridY);
                openList.Add(item);
            }
        }

        return false;
    }

    List<AstarNode> GenerateRote(AstarNode curNode)
    {
        rodeList = new List<AstarNode>();
        rodeList.Add(curNode);
        while (curNode.parent != null)
        {
            curNode = curNode.parent;
            rodeList.Add(curNode);
        }
        rodeList.Reverse();
        return rodeList;
    }

    float GetDestance(AstarNode node1, AstarNode node2)
    {
        float distance  ;
        if (node1.gridX != node2.gridX && node1.gridY != node2.gridY)
        {
            distance = 1.4f;
        }
        else
        {
            distance = 1f;
        }
        return distance; 
    }
}

控制对象在路径点移动

using System.Collections.Generic;
using UnityEngine;

public class AstartMoveTest : MonoBehaviour
{
    public Transform moveTarget;//移动对象
    private List<AstarNode> rodeNodeList;//行走路径点
    private int moveIndex;//当前移动到第几个点
    private bool isMoveing = false;//是否正在移动过程中
    private float miniDistance = 0.1f;
    private Vector3 endPosition;
    private Vector3 moveDir;
    public float speed = 1;
    // Start is called before the first frame update
    void Start()
    {
        
    }

    public void BeginMove(List<AstarNode> rodeNodeList)
    {
        if (rodeNodeList == null || rodeNodeList.Count < 2)
        {
            Debug.LogError("路径点异常，无法开始移动");
            return;
        }

        
        this.rodeNodeList = rodeNodeList;
        transform.position = rodeNodeList[0].position;
        isMoveing = true;
        moveIndex = 1;
        endPosition = rodeNodeList[moveIndex].position;
        moveDir = rodeNodeList[moveIndex].position - rodeNodeList[0].position;
        moveDir = moveDir.normalized;
    }

    // Update is called once per frame
    void Update()
    {
        if (!isMoveing)
        {
            return;
        }

        float distance = Vector3.Distance(transform.position, endPosition);
        // Debug.Log("distance = " + distance);
        if (distance < miniDistance)
        {
            moveIndex++;
            if (moveIndex >= rodeNodeList.Count)
            {
                isMoveing = false;
                Debug.Log("移动结束");
                return;
            }

            endPosition = rodeNodeList[moveIndex].position;
            transform.LookAt(endPosition);
            moveDir = endPosition - transform.position;
            moveDir = moveDir.normalized;
        }

        // transform.position = Vector3.Lerp(transform.position, endPosition, speed*Time.deltaTime);
        transform.position = transform.position + moveDir * Time.deltaTime * speed;
    }
}

网格是用Gizmos中绘制的，需要调整为top视图
图中黄色网格是终点，灰色网格是起始点，白色是寻路结果，红色是障碍物点

资源下载地址 demo链接