/// <summary>
/// 路径规划
/// </summary>
/// <param name="snId">起始点编号</param>
/// <param name="gnId">目标点编号</param>
/// <returns>规划所得路径的节点列表</returns>
public StoreHeap PathPanning(int snId, int gnId)
{
if (snId != gnId)
{
Node startNode = NodeList.FindNode(snId); //起始点
Node goalNode = NodeList.FindNode(gnId); //目标点
// 设置起始点和目标点的累积代价为0
startNode.CurrentCost = goalNode.CurrentCost = 0;
foreach (Node node1 in NodeList)
{
node1.NGoal = goalNode;
}
// 添加起始点到开放集合
OpenSet.Add(startNode);
DataTable dt = Graph.DS.Tables["NodeInfo"];
while (true)
{
// 当前点为开放集合中启发代价最小的点
Node currentNode = OpenSet.GetMinCostNode();
// 如果当前点为目标点，则循环结束
if (currentNode.Index == goalNode.Index)
{
// 设置目标点的父节点为关闭集合中的最后一个点
goalNode.NParent = ClosedSet.Pop();
break;
}
// 将当前点从开放集合中删去并添加到关闭集合
OpenSet.Remove(currentNode);
ClosedSet.Add(currentNode);
for (int i = 0; i < Graph.Nodes; i++)
{
if (Graph.Matrix[currentNode.Index - 1, i] != 0)
{
Node node = NodeList.FindNode((int)dt.Rows[i]["id"]);
node.CurrentCost = currentNode.CurrentCost + Graph.Matrix[currentNode.Index - 1, i];
// 如果节点存在于关闭集合，则跳过后续步骤，直接下一个循环
if (ClosedSet.IsExist(node))
{
continue;
}
// 如果节点不在开放集合，则将节点加入到开放集合
else if (!OpenSet.IsExist(node))
{
node.NParent = currentNode;
OpenSet.Add(node);
}
else
{
// 如果当前点到查询到的点的代价比现在的大，则更新节点的父节点
if (OpenSet.FindNode(node.Index).CurrentCost >= node.CurrentCost)
{
node.NParent = currentNode;
OpenSet.ReplaceNode(node);
}
}
}
}
}
StoreHeap finalPath = new StoreHeap(); //最终路径的节点列表
Node recallnode = goalNode;
finalPath.Add(recallnode);
// 只要当前点还存在父节点，则将其都加入到最终路径的节点列表
while (recallnode.HasParent())
{
recallnode = recallnode.NParent;
finalPath.Add(recallnode);
}
finalPath.Reverse();
return(finalPath);
}
// 如果起始点和目标点为同一点，则返回一个包含起始点的节点列表
else
{
StoreHeap result = new StoreHeap();
result.Add(NodeList.FindNode(snId));
return(result);
}
}
