/// <summary>
/// 使用A*算法依据当前信息计算一条最短的路径。
/// 注意，如果目标点在警戒线以内，会返回一条并非如你期望的路径。
/// 所以请自行实现目标点无法到达时的处理逻辑。
/// </summary>
/// <param name="curPos"></param>
/// <param name="aimPos"></param>
/// <returns></returns>
public NaviPoint[] CalPathUseAStar(Vector2 curPos, Vector2 aimPos)
{
#region 制导航图
GraphPoint <NaviPoint>[] map = new GraphPoint <NaviPoint> [Map.Length + 2];
GraphPoint <NaviPoint>[] temp = GraphPoint <NaviPoint> .DepthCopy(Map);
for (int i = 0; i < temp.Length; i++)
{
map[i] = temp[i];
}
#endregion
#region 将当前点和目标点加入到导航图中
int prePointSum = temp.Length;
GraphPoint <NaviPoint> curNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, curPos), new List <GraphPath <NaviPoint> >());
GraphPoint <NaviPoint> aimNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, aimPos), new List <GraphPath <NaviPoint> >());
AddCurPosToNaviMap(map, curNaviPoint, prePointSum, GuardLines, BorderLines);
AddAimPosToNaviMap(map, aimNaviPoint, curNaviPoint, prePointSum, GuardLines, BorderLines);
#endregion
#region 计算最短路径，使用A*算法
List <NodeAStar> open = new List <NodeAStar>();
List <NodeAStar> close = new List <NodeAStar>();
open.Add(new NodeAStar(curNaviPoint, null));
NodeAStar cur = null;
while (open.Count != 0)
{
cur = open[open.Count - 1];
if (cur.point == aimNaviPoint)
{
break;
}
open.RemoveAt(open.Count - 1);
close.Add(cur);
foreach (GraphPath <NaviPoint> path in cur.point.neighbors)
{
if (Contains(close, path.neighbor))
{
continue;
}
else
{
NodeAStar inOpenNode;
if (Contains(open, path.neighbor, out inOpenNode))
{
float G = cur.G + path.weight;
if (inOpenNode.G > G)
{
inOpenNode.G = G;
inOpenNode.F = G + inOpenNode.H;
}
}
else
{
NodeAStar childNode = new NodeAStar(path.neighbor, cur);
childNode.G = cur.G + path.weight;
childNode.H = Vector2.Distance(aimPos, childNode.point.value.Pos);
childNode.F = childNode.G + childNode.H;
SortInsert(open, childNode);
}
}
}
}
//if (cur == null)
// return null;
Stack <NodeAStar> cahe = new Stack <NodeAStar>();
while (cur.father != null)
{
cahe.Push(cur);
cur = cur.father;
}
NaviPoint[] result = new NaviPoint[cahe.Count];
int j = 0;
foreach (NodeAStar node in cahe)
{
result[j] = node.point.value;
j++;
}
return(result);
#endregion
}
