/// <summary>
/// 设置出边序号，调整边中格子的顺序
/// </summary>
/// <param name="listNavigateVertexNode">输入并输出参数, 顶点表示的路径,每个元素记录的是边对应结点的入边, 在函数内设置出边的序号</param>
/// <param name="listEdge">输出参数，路径上的边，顶点数减一条，其中的各格子按路过的顺序排列</param>
public void NormalizeVertexAndSpotsOrder(List <NavigateVertexNode> listNavigateVertexNode, List <Edge> listEdge)
{
listEdge.Clear();
NavigateVertexNode nvn, nvnPrevious = listNavigateVertexNode[0];
for (int i = 1; i < listNavigateVertexNode.Count; i++)
{
nvn = listNavigateVertexNode[i];
if (nvn.SelectedEdge.EdgeLink.Cell1 == nvn.Cell) // 顺序相反, 反转
{
nvn.SelectedEdge.EdgeLink.ReverseSpots();
}
listEdge.Add(nvn.SelectedEdge.EdgeLink);
// 设置前一路口之----过路口之后的边在该路口边表中的位置
nvnPrevious.NextSelectedEdgeIndex = this.GetEdgeNumber(nvnPrevious, nvn.SelectedEdge.EdgeLink);
nvnPrevious = nvn;
}
}
// 返回被删除的边的引用
/*private Edge RemoveEdgeNode(int vertexIndex, int adjacentIndex)
* {
* EdgeNode p = this.vertexList[vertexIndex].FirstEdge, q = null;
* while (p != null && p.AdjacentVertex != adjacentIndex)
* {
* q = p;
* p = p.Next;
* }
*
* if (p != null) // 找到
* {
* if (q == null) // 首条边
* {
* this.vertexList[vertexIndex].FirstEdge = p.Next;
* }
* else
* {
* q.Next = p.Next;
* }
*
* return p.EdgeLink;
* }
*
* return null;
* }*/
/// <summary>
/// 采用A*算法寻最短路
/// A*算法 (F=G+H)
/// 1，把起始格添加到开启列表。
/// 2，重复如下的工作：
/// a) 寻找开启列表中F值最低的格子。我们称它为当前格。
/// b) 把它切换到关闭列表。
/// c) 对相邻的格中的每一个
/// * 如果它不可通过或者已经在关闭列表中，略过它。反之如下。
/// * 如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。
/// * 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。
/// * 如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。
/// * 如果要保持的开启列表按F值排序，改变之后可能需要重新对开启列表排序。
/// d) 停止，当
/// * 把目标格添加进了关闭列表，这时候路径被找到，或者
/// * 没有找到目标格，开启列表已经空了。这时候，路径不存在。
/// 3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。这就是找到的路径。
/// </summary>
/// <param name="vnFrom">起点</param>
/// <param name="vnTo">终点</param>
/// <param name="listNavigateVertexNode">顶点表示的路径,每个元素记录的是边对应结点的入边, "并未"设置出边的序号</param>
/// <returns>路径的总权值, 负数表示没找到</returns>
public float FindShortestPaths(AStarHelper ash, VertexNode vnFrom, VertexNode vnTo, List <NavigateVertexNode> listNavigateVertexNode)
{
listNavigateVertexNode.Clear();
AStarHelper.SeekingNode me = new AStarHelper.SeekingNode(-1, 0.0f, 0.0f, null, vnFrom, null);
ash.InsertIntoOpening(me);
while (ash.OpeningLength > 0)
{
me = ash.PopLowestWeightInOpening();
VertexNode vn = me.Data as VertexNode;
if (vn.Cell == vnTo.Cell)
{
break; // 找到
}
// 遍历邻接点
EdgeNode p = vn.FirstEdge;
int whichAdjacent = 0;
while (p != null)
{
if (ash.IsInClosed(p.EdgeLink.ID))
{
p = p.Next;
whichAdjacent++;
continue;
}
NavigateVertexNode nvn = new NavigateVertexNode(this.vertexList[p.AdjacentVertex] as VertexNode);
nvn.SelectedEdgeIndex = whichAdjacent;
nvn.SelectedEdge = p;
float h = ash.hFunc(this.vertexList[p.AdjacentVertex].Cell, vnTo.Cell);
//string info = string.Format("edge={0:d}<->{1:d}, g={2:f}, h={3:f}",
// vn.Cell, this.vertexList[p.AdjacentVertex].Cell, me.G + p.EdgeLink.Weight, h);
//XxdwDebugger.Log(info);
AStarHelper.SeekingNode next = new AStarHelper.SeekingNode(
p.EdgeLink.ID,
me.G + p.EdgeLink.Weight,
h,
me,
this.vertexList[p.AdjacentVertex],
nvn);
AStarHelper.SeekingNode pre = ash.FindInOpening(p.EdgeLink.ID);
if (pre == null)
{
ash.InsertIntoOpening(next);
}
else if (next.F < pre.F)
{
ash.RemoveFromOpening(pre);
ash.InsertIntoOpening(next);
}
p = p.Next;
whichAdjacent++;
}
} // while
float weight = 0.0f;
if ((me.Data as VertexNode).Cell == vnTo.Cell) // 找到
{
NavigateVertexNode nvn;
AStarHelper.SeekingNode sn = me;
while (sn != null && sn.ExtraData != null)
{
nvn = sn.ExtraData as NavigateVertexNode;
listNavigateVertexNode.Insert(0, nvn);
weight += nvn.SelectedEdge.EdgeLink.Weight;
sn = sn.Parent;
}
listNavigateVertexNode.Insert(0, new NavigateVertexNode(vnFrom)); //加入开始节点
return(weight);
}
else
{
return(-1.0f);
}
}
