A*寻路

using UnityEngine;
using System.Collections.Generic;

public class TestXunLu : MonoBehaviour
{
//以点为方格
//A点(起点) [1,2] B点(终点)[6,7]
private List<float[,]> parentPosList = new List<float[,]>();
List<Son> sonList = new List<Son>();
List<Son> parentList = new List<Son>();
List<Son> oldList = new List<Son>();
//障碍物
List<float[,]> barrierPosList = new List<float[,]>();
//防止测试死机
private int n = 0;
private float[,] start = new float[1, 2] { { 1, 2 } };
private float[,] end = new float[1, 2] { { 6, 7 } };

void Start()
{
barrierPosList.Add(new float[,] { { 2, 4 } });
barrierPosList.Add(new float[,] { { 3, 4 } });
barrierPosList.Add(new float[,] { { 4, 4 } });

XunLu(start, end);
}


public void XunLu(float[,] start, float[,] end)
{
//找到起始点A 周围8个点 F=G+H G值由父点决定(决定周围8点) H值由终点B点决定
//需要一个存 父点 的list parent
//首先可以知道start点 周围8点坐标 存到list son 
//存父节点
//List<Son> parent = new List<Son>();
parentPosList.Add(start);
//===
sonList.Clear();
//拿到周围8个点坐标 
Son one = new Son(new float[1, 2] { { start[0, 0], start[0, 1] + 1 } });
Son two = new Son(new float[1, 2] { { start[0, 0] + 1, start[0, 1] + 1 } });
Son three = new Son(new float[1, 2] { { start[0, 0] + 1, start[0, 1] } });
Son four = new Son(new float[1, 2] { { start[0, 0] + 1, start[0, 1] - 1 } });
Son five = new Son(new float[1, 2] { { start[0, 0], start[0, 1] - 1 } });
Son six = new Son(new float[1, 2] { { start[0, 0] - 1, start[0, 1] - 1 } });
Son seven = new Son(new float[1, 2] { { start[0, 0] - 1, start[0, 1] } });
Son eight = new Son(new float[1, 2] { { start[0, 0] - 1, start[0, 1] + 1 } });
sonList.Add(one); sonList.Add(two); sonList.Add(three); sonList.Add(four); sonList.Add(five); sonList.Add(six); sonList.Add(seven); sonList.Add(eight);


n++;
for (int i = 0; i < sonList.Count; i++)
{
if (sonList[i].pos[0, 0] == end[0, 0] && sonList[i].pos[0, 1] == end[0, 1])
{
Debug.Log("路径已经找到!!");
Debug.Log("输出路径:");
for (int j = 0; j < parentPosList.Count; j++)
{
Debug.Log("路径坐标:" + parentPosList[j][0, 0] + " , " + parentPosList[j][0, 1]);
}
return;


}
}
if (n > 100)
{
Debug.Log("n>100 return!!!");
Debug.Log(parentPosList.Count);
return;
}




for (int i = 0; i < sonList.Count; i++)
{
sonList[i].H = (Mathf.Abs(end[0, 0] - sonList[i].pos[0, 0]) + Mathf.Abs(end[0, 1] - sonList[i].pos[0, 1])) * 10;//H
if (sonList[i].pos[0, 0] == start[0, 0] || sonList[i].pos[0, 1] == start[0, 1])
{
sonList[i].G = 10;
}
else
{
sonList[i].G = 14;
}
sonList[i].F = sonList[i].G + sonList[i].H;
}
for (int i = 0; i < sonList.Count; i++)
{
for (int j = 0; j < barrierPosList.Count; j++)
{
if (sonList[i].pos[0, 0] == barrierPosList[j][0, 0] && sonList[i].pos[0, 1] == barrierPosList[j][0, 1])
{
sonList[i].F = 1000;
}
}
}

for (int i = 0; i < sonList.Count - 1; i++)
{
if (sonList[i].F < sonList[i + 1].F)
{
sonList[i + 1] = sonList[i];
}
}

float[,] pos = sonList[7].pos;
XunLu(pos, end);
}


//Node
public class Son
{
public float F;
public float G;
public float H;
public float[,] pos;
public float[,] parentPos;
public Son(float[,] pos)
{
this.pos = pos;
}
}
}