猫要去骨头的地方
我们使用方块(一个正方形)作为寻路算法的单元
源码
//每个节点
class NodeData
{
public int X;
public int Y;
public NodeData preNode;
public int F;//到目的地估算值
}
//定义的数据
private int[,] map = new int[8, 9];
private Dictionary<int, NodeData> m_closeNodeDic = new Dictionary<int, NodeData>();
private Dictionary<int, NodeData> m_openNodeDic = new Dictionary<int, NodeData>();
//初始化map 以及开始查找
private void TestAStar()
{
//阻挡
for (int i = 0; i < map.GetLength(0); i++)
{
map[i, 0] = -1;
map[i, map.GetLength(1) -1] = -1;
}
for (int i = 0; i < map.GetLength(1); i++)
{
map[0, i] = -1;
map[map.GetLength(0) - 1, i] = -1;
}
//固定障碍物
map[2, 4] = -1;
map[3, 4] = -1;
map[4, 4] = -1;
map[5, 4] = -1;
map[5, 2] = -1;
for (int i = 0; i < map.GetLength(0); i++)
{
string str = "";
for (int j = 0; j < map.GetLength(1); j++)
{
if(string.IsNullOrEmpty(str))
{
str += map[i,j];
}
else
{
str += " " + map[i, j];
}
}
DLogger.Error(str);
}
NodeData startNode = new NodeData();
startNode.X = 4;
startNode.Y = 2;
FindPath(map,startNode,3,8);
}
private int GetDicKey(int x,int y)
{
return x * 10000 + y;
}
private int GetH(int curX, int curY, int targetX, int targetY)
{
return Mathf.Abs(targetX - curX)*10 + Mathf.Abs(targetY - curY)*10;
}
private void AddOpenList(int[,] map, NodeData startNode, int curX, int curY, int targetX, int targetY)
{
if (curX < 0 || curY < 0 || curX >= map.GetLength(0) || curY >= map.GetLength(1))
return;
//如果是障碍物
if (map[curX, curY] < 0)
return;
if (m_closeNodeDic.ContainsKey(GetDicKey(curX, curY)))
return;
int curH = GetH(curX,curY,targetX,targetY);
int curG = 10;
NodeData preNode = startNode.preNode;
while(preNode != null)
{
preNode = preNode.preNode;
curG += 10;
}
int curF = curG + curH;
NodeData nodeData;
if(m_openNodeDic.TryGetValue(GetDicKey(curX,curY), out nodeData))
{
//更新F
if(nodeData.F < curF)
{
nodeData.preNode = startNode;
nodeData.F = curF;
}
}
else
{
nodeData = new NodeData();
nodeData.X = curX;
nodeData.Y = curY;
nodeData.preNode = startNode;
nodeData.F = curF;
m_openNodeDic.Add(GetDicKey(curX, curY), nodeData);
}
}
private void FindPath(int[,] map,NodeData startNode, int targetX, int targetY)
{
m_openNodeDic.Remove(GetDicKey(startNode.X, startNode.Y));
m_closeNodeDic.Add(GetDicKey(startNode.X, startNode.Y), startNode);
AddOpenList(map, startNode, startNode.X - 1, startNode.Y,targetX, targetY);
AddOpenList(map, startNode, startNode.X + 1, startNode.Y, targetX, targetY);
AddOpenList(map, startNode, startNode.X, startNode.Y - 1, targetX, targetY);
AddOpenList(map, startNode, startNode.X, startNode.Y + 1, targetX, targetY);
//检查下是否已经到目的地了
NodeData targetNode;
if(m_openNodeDic.TryGetValue(GetDicKey(targetX,targetY), out targetNode))
{
while(targetNode.preNode != null)
{
DLogger.Error("X:{0} Y:{1}", targetNode.preNode.X, targetNode.preNode.Y);
targetNode = targetNode.preNode;
}
}
else
{
//没找到
if(m_openNodeDic.Count > 0)
{
//找到最小F值的node
Dictionary<int, NodeData> open_SortedByF = m_openNodeDic.OrderBy(o => o.Value.F).ToDictionary(p => p.Key, o => o.Value);
var itor = open_SortedByF.GetEnumerator();
itor.MoveNext();
FindPath(map, itor.Current.Value, targetX, targetY);
}
else
{
//到不了
DLogger.Error("目的地到不了 =================");
}
}
}