Unity C#实现A*算法
- 算法原理
简单来说就是把地图拆成N*N个格子 以每个格子为最小移动单位。知道起始点和终点。从起点开始遍历周围的格子。计算出他们的行走代价（F=G+H）；并加到待搜索列表中去。 然后不断的找待搜索列表中F代价最小的节点 找到最小F代价的节点加入到已搜索列表中。 再开始遍历他周围的格子更新代价值G,H和F。最终最后遍历到终点或者是待搜索列表没有数据 算法结束。
- F=G+H
F代表这个格子的总代价 这个值越小说明这个可能是当前最佳的路线格子。
G代表从起始格子到当前格子的行走消耗代价。
H代表当前格子到终点的行走消耗代价
一般计算方法为 曼哈顿距离（当前格子到终点的水平X距离 + 当前格子到终点的垂直Y的距离 即：curNode.gCost = Mathf.Abs(curNode.x - endNode.x) + Mathf.Abs(curNode.y - endNode.y)）
另一种可以走对角线的计算方法为 切比雪夫距离(定义水平和垂直距离移动一格消耗10，对角线方向移动一格消耗为 14 = 即根号2取的整 乘10 避免小数运算)
int offsetX = Mathf.Abs(curNode.posX - endNode.posX);
int offsetY = Mathf.Abs(curNode.poxY - endNode.poxY);
int offsetDis = Mathf.Abs(offsetX - offsetY); //差值 为水平距离 例如 offsetX = 3 offsetY =1 就是水平走2格 斜着1格子
int maxOffset = offsetX > offsetY ? offsetX : offsetY; //找最大 最大值减去插值认为是斜边距离
curNode.gCost = offsetDis * 10+ (maxOffset - offsetDis) * 14;
- 注意在遍历周围格子时 需要判断他是否是在待搜索列表 如果在 就需要尝试计算一下当前点到这个周围格子的点的 gCost 如果比他原来记录的要小 那么就要更新一下他的gCost 和他的来源节点 赋值为当前节点。 这样就相当于更新了最佳路径
具体细节之间上代码把
先看Node.cs
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Node : MonoBehaviour
{
// G cost 代表从起点到这个节点的耗费
public int gCost = 0;
// H cost 代表这个节点到终点的 曼哈顿距离 就是 X+Y
public int hCost = 0;
public int fCost = 0;
public int posX;
public int poxY;
//F cost = G cost + H cost
public void FCost()
{
fCost = hCost + gCost;
}
//来源父节点
public Node parentNode = null;
//是否是障碍
public bool isWall = false;
[ContextMenu("SetWall 设置为墙体")]
public void SetWall()
{
MapManager.Instance.SetNodeWall(this.gameObject);
}
public string toString()
{
return "x: " + posX + "|y: " + poxY;
}
}
MapManger.cs
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class MapManager : MonoBehaviour
{
public static MapManager _instance;
const int ONE_PATH_COST = 10; //水平或者垂直移动一格消耗
const int ONE_INCLIEND_PATH_COST = 14; //斜方向移动一格消耗
[Header("地图高")]
public const int hight = 20;
[Header("地图宽")]
public const int widget = 20;
[Header("格子")]
public GameObject nodeGameObj = null;
[Header("墙 shader")]
public Material wallMaterial = null;
public Node[,] allNodes = new Node[hight, widget];
public static MapManager Instance
{
get {
return _instance;
}
}
public void Awake()
{
_instance = this;
}
public void Start()
{
CreateMap();
}
public void CreateMap()
{
for (int i = 0; i < widget; i++)
{
for(int j = 0; j < hight; j++)
{
//创建node
GameObject nodeObj = GameObject.Instantiate<GameObject>(nodeGameObj);
Vector3 pos = new Vector3(i, j, 0);
nodeObj.transform.position = pos;
nodeObj.gameObject.name = i + "_" + j;
Node nodeComponent = nodeObj.AddComponent<Node>();
nodeComponent.posX = i;
nodeComponent.poxY = j;
allNodes[i,j] = nodeComponent;
}
}
}
public void InitMapCost()
{
for (int i = 0; i < widget; i++)
{
for (int j = 0; j < hight; j++)
{
//重置一下 避免二次触发死循环
allNodes[i, j].gCost = int.MaxValue;
allNodes[i, j].hCost = 0;
allNodes[i, j].parentNode = null;
allNodes[i, j].FCost();
}
}
}
public Node GetNode(int x,int y)
{
return allNodes[x, y];
}
//计算两个点之间的距离耗费
public int CalculTowNodeDistance(Node startNode, Node endNode)
{
int offsetX = Mathf.Abs(startNode.posX - endNode.posX);
int offsetY = Mathf.Abs(startNode.poxY - endNode.poxY);
int offsetDis = Mathf.Abs(offsetX - offsetY); //差值 为水平距离 例如 x:3 y:1 就是水平走2格 斜着1格子
int maxOffset = offsetX > offsetY ? offsetX : offsetY; //找最大 最大值减去插值认为是斜边距离
return offsetDis * ONE_PATH_COST + (maxOffset - offsetDis) * ONE_INCLIEND_PATH_COST;
}
public List<Node> GetNeighborNodeList(Node node)
{
//拿到相邻节点
List<Node> neighborNodeList = new List<Node>();
int curPox = node.posX;
int curPoy = node.poxY;
int[,] neighborIndexs = new int[,]
{
{
0,1 }, //上
{
0,-1}, //下
{
-1,0 }, //左
{
1,0 }, //右
{
-1,1 }, //左上
{
1,1 }, //右上
{
-1,-1 }, //左下
{
1,-1 }, //右下
};
for(int i = 0; i< 8; i++)
{
int offsetX = neighborIndexs[i,0];
int offsetY = neighborIndexs[i, 1];
int realIndexX = curPox + offsetX;
int realIndexY = curPoy + offsetY;
if(realIndexX < 0 || realIndexX >= widget)
{
continue;
}
if (realIndexY < 0 || realIndexY >= hight)
{
continue;
}
neighborNodeList.Add(GetNode(realIndexX, realIndexY));
}
return neighborNodeList;
}
public void SetNodeWall(GameObject obj)
{
Node nodeComponet = obj.GetComponent<Node>();
nodeComponet.isWall = true;
MeshRenderer mr = obj.GetComponent<MeshRenderer>();
mr.material = wallMaterial;
}
}
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class AStarFind : MonoBehaviour
{
public Material startMat;
public Material endMat;
public Material nomarlMat;
public Material wallMaterial;
public GameObject startObj;
public GameObject endObj;
public List<Node> findPathNodeList = null;
public List<Node> FindAStartNodePath(Node startNode, Node endNode)
{
List<Node> toSearchList = new List<Node>() {
startNode }; //待搜索列表
List<Node> findedList = new List<Node>(); //已经搜索过的列表
//初始化一波地图消耗
MapManager.Instance.InitMapCost();
Node curNode = null;
while(toSearchList.Count > 0)
{
//有待搜索的 就一直执行
if(curNode == null)
{
curNode = toSearchList[0];
toSearchList.Remove(curNode);
}else
{
//找到当前待搜索列表里最小的F
toSearchList.Sort((a, b) =>
{
var cost = a.fCost - b.fCost;
return cost;
});
curNode = toSearchList[0];
toSearchList.Remove(curNode);
}
if(curNode.Equals(endNode))
{
//到终点了 返回找过的列表
//倒叙找回起点 就是路径了
Debug.Log("term tips 找到路径了！！！");
Node curParentNode = endNode.parentNode;
List<Node> getPathNodeList = new List<Node>();
while(curParentNode != null)
{
getPathNodeList.Add(curParentNode);
curParentNode = curParentNode.parentNode;
}
//反转一下 就是路径了
getPathNodeList.Reverse(); //反转函数
return getPathNodeList;
}
findedList.Add(curNode); //加到已搜索列表
//上下左右四个方向找一下 最佳的点
List <Node> neighborNodeList = MapManager.Instance.GetNeighborNodeList(curNode);
for(int i = 0; i < neighborNodeList.Count; i++)
{
Node curNeighborNode = neighborNodeList[i];
if(curNeighborNode.isWall)
{
//障碍
continue;
}
if(findedList.Contains(curNeighborNode))
{
//已经找过了
continue;
}
//尝试计算一下当前的点跟邻居节点的耗费
int curGCost = curNode.gCost + MapManager.Instance.CalculTowNodeDistance(curNode, curNeighborNode);
int curHCost = MapManager.Instance.CalculTowNodeDistance(curNeighborNode, endNode);
int curFCost = curGCost + curHCost;
if (!toSearchList.Contains(curNeighborNode))
{
//加到待搜索列表 同时算一下当前耗费
curNeighborNode.gCost = curGCost;
curNeighborNode.hCost = curHCost;
curNeighborNode.FCost();
curNeighborNode.parentNode = curNode;
toSearchList.Add(curNeighborNode);
}
else
{
//在待搜索列表里 比较一下Gcost 是不是最佳的 相当于回溯 之前的路径来源舍弃
if (curGCost < curNeighborNode.gCost)
{
//当前位置移动到这里更好 更新一下消耗
curNeighborNode.gCost = curGCost;
curNeighborNode.hCost = curHCost;
curNeighborNode.FCost();
curNeighborNode.parentNode = curNode;
}
}
}
}
Debug.Log("term tips 找不到路径了 - -！");
return null;
}
// Update is called once per frame
void Update()
{
if (Input.GetMouseButtonDown(0))
{
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit raycastHit;
Physics.Raycast(ray, out raycastHit, 1000);
if (raycastHit.collider)
{
GameObject hitObj = raycastHit.collider.gameObject;
if (startObj == null || startObj.Equals(hitObj))
{
startObj = hitObj;
startObj.GetComponent<MeshRenderer>().material = startMat;
}
else if (endObj == null)
{
endObj = hitObj;
endObj.GetComponent<MeshRenderer>().material = endMat;
//开始寻路
Node startNode = startObj.GetComponent<Node>();
Node endNode = endObj.GetComponent<Node>();
findPathNodeList = FindAStartNodePath(startNode, endNode);
if (findPathNodeList != null)
{
//打印出来吧
foreach (var node in findPathNodeList)
{
node.GetComponent<MeshRenderer>().material = startMat;
Debug.Log("path :" + node.toString());
}
}
}
}
}
else if (Input.GetMouseButton(1))
{
//设置墙体
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit raycastHit;
Physics.Raycast(ray, out raycastHit, 1000);
if (raycastHit.collider)
{
GameObject hitObj = raycastHit.collider.gameObject;
hitObj.GetComponent<Node>().isWall = true;
hitObj.GetComponent<MeshRenderer>().material = wallMaterial;
}
}
}
[ContextMenu("ClearStartAndEndPos 清空")]
public void ClearStartAndEndPos()
{
if(startObj)
{
startObj.GetComponent<MeshRenderer>().material = nomarlMat;
startObj = null;
}
if (endObj)
{
endObj.GetComponent<MeshRenderer>().material = nomarlMat;
endObj = null;
}
if (findPathNodeList != null)
{
//打印出来吧
foreach (var node in findPathNodeList)
{
node.GetComponent<MeshRenderer>().material = nomarlMat;
}
findPathNodeList = null;
}
}
}
最终效果
找到路径了
没有路径的情况：
