简易的A*算法 自动寻路
参考：
路径计算方式（详见参考：堪称最好最全的A*算法详解（译文））：
1. 曼哈顿距离，横向和纵向直线距离，仅限于横向纵向移动
2. 对角线距离，对角线 + 直线，可以横向、纵向、对角线方向移动
3. 欧几里得距离，任意角度直线，任意方向移动
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 }
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(); } }