/*
* OPEN //开放列表
* CLOSED //封闭列表
* 把开始节点加入开放列表
*
* loop
* //找出开放接列表F代价最小的节点
* current = node in OPEN with the lowest f_cost
* remove current from OPEN
* add current to CLOSED
*
* if current is the target node //path has been found
* return
*
* // 遍历所有邻居
* foreach neighbour of the current node
* // 如果邻居已经遍历过了，或者邻居已经在封闭列表就跳过
* if neighbour is not traversable or neighbour is in CLOSED
* skip to the next neighbour
* // 如果到这个邻居的路径更短或者这个邻居不在开发列表
* if new path to neighbour is shorter OR neighbour is not in OPEN
* set f_cost of neighbour
* set parent of neighbour to current
* if neighbour is not in OPEN
* add neighbour to OPEN
*/
#endregion
/// <summary>
/// 输入起始点和结束点，输出路径列表
/// A星算法核心公式就是F值的计算：
/// F = G + H
/// F - 方块的总移动代价
/// G - 开始点到当前方块的移动代价
/// H - 当前方块到结束点的预估移动代价
/// </summary>
public static List <GridItem> FindPath_AStar(PathFindManager mgr, GridItem start, GridItem end)
{
sw.Start();
bool pathSuccess = false;
openSet = new List <GridItem>();
closeSet = new HashSet <GridItem>();
openSet.Add(start);
while (openSet.Count > 0)
{
var current = openSet[0];
//找出开放列表中F代价最小的节点 todo 优化==》最优队列、或者最小堆
foreach (var item in openSet)
{
if (item.fCost < current.fCost)
{
current = item;
}
}
openSet.Remove(current);
closeSet.Add(current);
if (current == end)
{
sw.Stop();
Debug.Log("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
var neighbors = mgr.GetNeighbors(current);
foreach (var neighbor in neighbors)
{
// 剔除封闭列表元素
if (closeSet.Contains(neighbor) || !neighbor.walkable)
{
continue;
}
// 逐个计算邻居的gCost
int newCostToNeighbour = current.gCost + GetDistance(current, neighbor);
if (newCostToNeighbour < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newCostToNeighbour;
neighbor.hCost = GetDistance(end, neighbor);
neighbor.parent = current;//做成一个链表，最后用来做结果
neighbor.SetText(string.Format("f:{0}\ng:{1}\nh:{2}", neighbor.fCost, neighbor.gCost, neighbor.hCost));
if (openSet.Contains(neighbor))
{
//openSet
}
else
{
openSet.Add(neighbor);
}
}
}
}
if (pathSuccess)
{
var results = RetracePath(start, end);
pathSuccess = results.Count > 0;
return(results);
}
return(null);
}