//每次寻径之前根据起点、终点信息做初始化
bool Init(IPosition_Connected end, IPosition_Connected start)
{
//判断起点和终点是否在地图上，并初始化起点的标签
Tag tag = (Tag)end.GetAttachment();
//生成这次寻径使用的time stamp
time_stamp = TimeStamp.getNextTimeStamp(time_stamp);
if (tag != null)
{
tag.g = 0;
tag.parent = null;
tag.closed = false;
tag.timeStamp = time_stamp;
}
else
{
return(false);
}
if (start.GetAttachment() == null)
{
return(false);
}
return(true);
}
IPart_Connected GetParentPart(IPosition_Connected position_Connected, bool isPart)
{
if (isPart)
{
return(((IPart_Connected)position_Connected).GetParentPart());
}
else
{
return(((Tag_M2M_Position)position_Connected.GetAttachment()).parentPart);
}
}
void SetParentPart(IPosition_Connected position_Connected, IPart_Connected parentPart, bool isPart)
{
if (isPart)
{
((IPart_Connected)position_Connected).SetParentPart(parentPart);
}
else
{
((Tag_M2M_Position)position_Connected.GetAttachment()).parentPart = parentPart;
total++;
}
}
//没有路径则返回null
public List <IPosition_Connected> SearchPath(IPosition_Connected start, IPosition_Connected end)
{
path.Clear();
if (end == start)
{
path.Add(end);
return(path);
}
//初始化
if (!Init(end, start))
{
return(null);
}
IPosition_Connected p = null, p_adj;
IPositionSet_Connected_Adjacency adj_set;
Tag p_tag, p_adj_tag;
float newG;
//int count = 0;
//if (debug)
// Console.WriteLine("SearchPath:");
open.clear();
//将起点加入open表
open.add(end);
//当open表非空时
while (open.getSize() > 0)
{
//获得下一个最近的点
p = open.removeFirst();
//到达终点则结束
if (p == start)
{
pathLength = ((Tag)start.GetAttachment()).g;
break;
}
p_tag = (Tag)p.GetAttachment();
p_tag.closed = true;
adj_set = p.GetAdjacencyPositionSet();
adj_set.InitToTraverseSet();
//if (debug)
//{
// count++;
// Console.Write(p.ToString() + "\t");
//}
while (adj_set.NextPosition())
{
p_adj = adj_set.GetPosition_Connected();
p_adj_tag = (Tag)p_adj.GetAttachment();
//如果未被搜索
if (p_adj_tag.timeStamp != time_stamp)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
p_adj_tag.parent = p;
p_adj_tag.g = newG;
p_adj_tag.closed = false;
p_adj_tag.timeStamp = time_stamp;
open.add(p_adj);
}
else
{
if (!p_adj_tag.closed)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
if (newG < p_adj_tag.g)
{
p_adj_tag.parent = p;
p_adj_tag.g = newG;
open.update(p_adj);
}
}
}
//if (debug)
// Console.Write(p_adj.ToString() + ":" + p_adj.GetAttachment().ToString() + "\t");
}
//if (debug)
// Console.WriteLine();
}
//if (debug)
//{
// Console.Write("position count:");
// Console.WriteLine(count);
//}
//从终点根据标签中记录的父节点找到起点，生成路径
if (p == start)
{
path.Add(start);
p_tag = (Tag)start.GetAttachment();
while (p_tag.parent != null)
{
path.Add(p_tag.parent);
p_tag = (Tag)p_tag.parent.GetAttachment();
}
//path.Reverse();
return(path);
}
else
{
return(null);
}
}
//没有路径则返回null
public List <IPosition_Connected> SearchPath(IPosition_Connected start, IPosition_Connected end)
{
IPart_Connected startParentPart = ((Tag_M2M_Position)(start.GetAttachment())).parentPart;
IPart_Connected endParentPart = ((Tag_M2M_Position)(end.GetAttachment())).parentPart;
ushort lastLevelTimeStamp = TimeStamp.getRandomTimeStamp();
ushort endTime_stamp = TimeStamp.getRandomTimeStamp();
ushort startTime_stamp = TimeStamp.getRandomTimeStamp();
//从最上层的下一层开始进行搜索空间收缩
for (int levelSequence = 1; levelSequence < m2mStructure.GetLevelNum(); levelSequence++)
{
IPart_Connected currentLevelAncestorPartOfStartPosition = startParentPart;
IPart_Connected currentLevelAncestorPartOfEndPosition = endParentPart;
for (int i = 0; i < m2mStructure.GetLevelNum() - levelSequence - 1; i++)
{
currentLevelAncestorPartOfStartPosition = (IPart_Connected)currentLevelAncestorPartOfStartPosition.GetParentPart();
currentLevelAncestorPartOfEndPosition = (IPart_Connected)currentLevelAncestorPartOfEndPosition.GetParentPart();
}
Tag_M2M_Part StartPartTag = (Tag_M2M_Part)currentLevelAncestorPartOfStartPosition.GetAttachment();
StartPartTag.isNeedToSearch = true;
Tag_M2M_Part EndPartTag = (Tag_M2M_Part)currentLevelAncestorPartOfEndPosition.GetAttachment();
EndPartTag.isNeedToSearch = true;
bool isTopLevel = false;
if (levelSequence == 1)
{
isTopLevel = true;
}
//在该层中标记需要搜索的分块
{
//归入搜索空间的路径的最长长度
float PathLengthBound = float.MaxValue;
endTime_stamp = TimeStamp.getNextTimeStamp(startTime_stamp);
EndPartTag.timeStamp = endTime_stamp;
EndPartTag.ge = 0;
open.clear();
((M2M_Part_TagComparer)com).state = 0;
//将起点加入open表
open.add(currentLevelAncestorPartOfEndPosition);
//当open表非空时
while (open.getSize() > 0)
{
//获得下一个最近的点
IPart_Connected currentPosition_Connected = (IPart_Connected)open.removeFirst();
Tag_M2M_Part TagOfCurrentPart_Connected = (Tag_M2M_Part)currentPosition_Connected.GetAttachment();
//当open表里面所有的点的g值都超出路径最大长度范围，则结束循环
if (TagOfCurrentPart_Connected.ge >= PathLengthBound)
{
break;
}
//到达终点则记录路径的长度，并计算出搜索范围
if (currentPosition_Connected == currentLevelAncestorPartOfStartPosition)
{
if (PathLengthBound == float.MaxValue)
{
PathLengthBound = CalculatePathLengthBound(m2mStructure.GetLevelNum(), levelSequence, TagOfCurrentPart_Connected.ge);
}
}
TagOfCurrentPart_Connected.isClose = true;
IPositionSet_Connected_Adjacency CurrentPartAdjSet = currentPosition_Connected.GetAdjacencyPositionSet();
CurrentPartAdjSet.InitToTraverseSet();
while (CurrentPartAdjSet.NextPosition())
{
IPart_Connected adjPosition = (IPart_Connected)CurrentPartAdjSet.GetPosition_Connected();
Tag_M2M_Part tagOfAdjPosition = (Tag_M2M_Part)adjPosition.GetAttachment();
bool isInSearchingBound = false;
if (isTopLevel)
{
isInSearchingBound = true;
}
else
{
Tag_M2M_Part parenPartTag = (Tag_M2M_Part)adjPosition.GetParentPart().GetAttachment();
if ((parenPartTag.timeStamp == lastLevelTimeStamp) && (parenPartTag.isNeedToSearch == true))
{
isInSearchingBound = true;
}
}
//如果属于搜索空间
if (isInSearchingBound)
{
//如果未被搜索
if (tagOfAdjPosition.timeStamp != endTime_stamp)
{
float newG = TagOfCurrentPart_Connected.ge + CurrentPartAdjSet.GetDistanceToAdjacency();
if (newG < PathLengthBound)
{
tagOfAdjPosition.ge = newG;
tagOfAdjPosition.isClose = false;
tagOfAdjPosition.timeStamp = endTime_stamp;
open.add(adjPosition);
}
}
else
{
if (!tagOfAdjPosition.isClose)
{
float newG = TagOfCurrentPart_Connected.ge + CurrentPartAdjSet.GetDistanceToAdjacency();
if (newG < tagOfAdjPosition.ge)
{
tagOfAdjPosition.ge = newG;
open.update(adjPosition);
}
}
}
}
}
}
startTime_stamp = TimeStamp.getNextTimeStamp(endTime_stamp);
StartPartTag.timeStamp = startTime_stamp;
StartPartTag.gs = 0;
open.clear();
((M2M_Part_TagComparer)com).state = 1;
//将起点加入open表
open.add(currentLevelAncestorPartOfStartPosition);
//当open表非空时
while (open.getSize() > 0)
{
//获得下一个最近的点
IPart_Connected currentPosition_Connected = (IPart_Connected)open.removeFirst();
Tag_M2M_Part TagOfCurrentPart_Connected = (Tag_M2M_Part)currentPosition_Connected.GetAttachment();
//当open表里面所有的点的g值都超出路径最大长度范围，则结束循环
if (TagOfCurrentPart_Connected.gs >= PathLengthBound)
{
break;
}
TagOfCurrentPart_Connected.isClose = true;
IPositionSet_Connected_Adjacency CurrentPartAdjSet = currentPosition_Connected.GetAdjacencyPositionSet();
CurrentPartAdjSet.InitToTraverseSet();
while (CurrentPartAdjSet.NextPosition())
{
IPart_Connected adjPosition = (IPart_Connected)CurrentPartAdjSet.GetPosition_Connected();
Tag_M2M_Part tagOfAdjPosition = (Tag_M2M_Part)adjPosition.GetAttachment();
bool isInSearchingBound = false;
if (isTopLevel)
{
isInSearchingBound = true;
}
else
{
Tag_M2M_Part parenPartTag = (Tag_M2M_Part)adjPosition.GetParentPart().GetAttachment();
if ((parenPartTag.timeStamp == lastLevelTimeStamp) && (parenPartTag.isNeedToSearch == true))
{
isInSearchingBound = true;
}
}
//如果属于搜索空间
if (isInSearchingBound)
{
//如果未被搜索
if (tagOfAdjPosition.timeStamp != startTime_stamp)
{
float newG = TagOfCurrentPart_Connected.gs + CurrentPartAdjSet.GetDistanceToAdjacency();
if (newG < PathLengthBound)
{
tagOfAdjPosition.gs = newG;
tagOfAdjPosition.isClose = false;
//如果在上面的搜索过程中被搜索过
if (tagOfAdjPosition.timeStamp == endTime_stamp)
{
//如果两次搜索的g值之和小于路径的最大长度则标记该分块为下一层的待搜索空间
if (tagOfAdjPosition.ge + newG < PathLengthBound)
{
tagOfAdjPosition.isNeedToSearch = true;
}
else
{
tagOfAdjPosition.isNeedToSearch = false;
}
}
else
{
//如果在上面的搜索过程中没有被搜索过，则把它的ge值设大，使得以后改变g值的时候不会因为两个g值之和小于PathLengthBound而把该点标记为待搜索点
tagOfAdjPosition.ge = PathLengthBound + 1;
tagOfAdjPosition.isNeedToSearch = false;
}
tagOfAdjPosition.timeStamp = startTime_stamp;
open.add(adjPosition);
}
}
else
{
if (!tagOfAdjPosition.isClose)
{
float newG = TagOfCurrentPart_Connected.gs + CurrentPartAdjSet.GetDistanceToAdjacency();
if (newG < tagOfAdjPosition.gs)
{
if (tagOfAdjPosition.isNeedToSearch == false)
{
//如果两次搜索的g值之和小于路径的最大长度则标记该分块为下一层的待搜索空间
if (tagOfAdjPosition.ge + newG < PathLengthBound)
{
tagOfAdjPosition.isNeedToSearch = true;
}
}
tagOfAdjPosition.gs = newG;
open.update(adjPosition);
}
}
}
}
}
}
lastLevelTimeStamp = startTime_stamp;
if (GetTimeStamp != null)
{
GetTimeStamp(lastLevelTimeStamp);
}
}
}
if (GetM2MStructure != null)
{
GetM2MStructure(m2mStructure);
}
//最底层寻找真实路径
path.Clear();
((M2M_Part_TagComparer)com).state = 2;
if (end == start)
{
path.Add(end);
return(path);
}
//判断起点和终点是否在地图上，并初始化起点的标签
Tag_M2M_Position tag = (Tag_M2M_Position)end.GetAttachment();
//生成这次寻径使用的time stamp
time_stamp = TimeStamp.getNextTimeStamp(lastLevelTimeStamp);
if (tag != null)
{
tag.g = 0;
tag.parent = null;
tag.isClose = false;
tag.timeStamp = time_stamp;
}
IPosition_Connected p = null, p_adj;
IPositionSet_Connected_Adjacency adj_set;
Tag_M2M_Position p_tag, p_adj_tag;
open.clear();
//将起点加入open表
open.add(end);
//当open表非空时
while (open.getSize() > 0)
{
float newG;
//获得下一个最近的点
p = open.removeFirst();
//到达终点则结束
if (p == start)
{
pathLength = ((Tag_M2M_Position)start.GetAttachment()).g;
break;
}
p_tag = (Tag_M2M_Position)p.GetAttachment();
p_tag.isClose = true;
adj_set = p.GetAdjacencyPositionSet();
adj_set.InitToTraverseSet();
while (adj_set.NextPosition())
{
p_adj = adj_set.GetPosition_Connected();
p_adj_tag = (Tag_M2M_Position)p_adj.GetAttachment();
//如果需要搜索
Tag_M2M_Part parentPartTag = (Tag_M2M_Part)p_adj_tag.parentPart.GetAttachment();
if (parentPartTag.isNeedToSearch && parentPartTag.timeStamp == lastLevelTimeStamp)
{
//如果未被搜索
if (p_adj_tag.timeStamp != time_stamp)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
p_adj_tag.parent = p;
p_adj_tag.g = newG;
p_adj_tag.isClose = false;
p_adj_tag.timeStamp = time_stamp;
open.add(p_adj);
}
else
{
if (!p_adj_tag.isClose)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
if (newG < p_adj_tag.g)
{
p_adj_tag.parent = p;
p_adj_tag.g = newG;
open.update(p_adj);
}
}
}
}
}
}
//从终点根据标签中记录的父节点找到起点，生成路径
if (p == start)
{
path.Add(start);
p_tag = (Tag_M2M_Position)start.GetAttachment();
while (p_tag.parent != null)
{
path.Add(p_tag.parent);
p_tag = (Tag_M2M_Position)p_tag.parent.GetAttachment();
}
//path.Reverse();
return(path);
}
else
{
return(null);
}
}
private void pathFindingToolStripMenuItem_Click(object sender, EventArgs e)
{
IPositionSet_Connected positionSet_Connected = (IPositionSet_Connected)positionSetContainer.GetPositionSet();
Dijkstra dijkstra = new Dijkstra();
AStar astar = new AStar();
M2M_PF m2m_PF = new M2M_PF();
IM2MStructure m2mStructure = null;
List <IPosition_Connected> m2mPath = null;
List <IPosition_Connected> path = null;
//计算算法运行时间用
//IPosition_Connected start = null;
//IPosition_Connected end = null;
ISearchPathEngine searchPathEngine1 = m2m_PF;
ISearchPathEngine searchPathEngine2 = dijkstra;
m2m_PF.GetM2MStructure += delegate(IM2MStructure m2mS) { m2mStructure = m2mS; };
m2m_PF.GetM2MStructureInPreprocess += delegate(IM2MStructure m2mS) { m2mStructure = m2mS; };
List <ushort> timeStampList = new List <ushort>();
m2m_PF.GetTimeStamp += delegate(ushort timeStamp) { timeStampList.Add(timeStamp); };
searchPathEngine1.InitEngineForMap(positionSet_Connected);
searchPathEngine2.InitEngineForMap(positionSet_Connected);
//以下代码必须在UI线程中调用，即不能在另开的线程中调用
LayersExOptDlg layers = new LayersExOptDlg();
LayersPainterForm layersPainterForm = new LayersPainterForm(layers);
LayersPaintedControl layersPaintedControl = layersPainterForm.LayersPaintedControl;
LayersEditedControl layersEditedControl = new LayersEditedControl();
layersEditedControl.Dock = DockStyle.Top;
layersEditedControl.LayersPaintedControl = layersPaintedControl;
layersPainterForm.Controls.Add(layersEditedControl);
layersPainterForm.Show();
//打开一个Worker线程来进行算法流程的演示（否则会阻塞UI线程以至于演示不能进行）
IAsyncResult result = new dDemoProcess(delegate
{
Layer_PositionSet_Connected layer_PositionSet_Connected = new Layer_PositionSet_Connected(positionSet_Connected);
layer_PositionSet_Connected.Point.PointColor = Color.Yellow;
layer_PositionSet_Connected.Connection.LineColor = Color.Blue;
layer_PositionSet_Connected.Connection.LineWidth = 0.6f;
layer_PositionSet_Connected.Point.PointRadius = 1.2f;
layers.Add(layer_PositionSet_Connected);
layer_PositionSet_Connected.SpringLayerRepresentationChangedEvent(layer_PositionSet_Connected);
for (int levelSequence = 1; levelSequence < m2mStructure.GetLevelNum(); levelSequence++)
{
Layer_PartSet_Connected layer_PartSet_Connected = new Layer_PartSet_Connected(m2mStructure, levelSequence);
layer_PartSet_Connected.Visible = false;
layers.Add(layer_PartSet_Connected);
}
while (true)
{
IPosition_Connected start = (IPosition_Connected)layersEditedControl.GetMouseDoubleChickedNearestPositionInCurrentPositionSet(positionSet_Connected);
PositionSet_ConnectedEdit startPointSet = new PositionSet_ConnectedEdit();
startPointSet.AddPosition_Connected(start);
Layer_PositionSet_Point layerStartPoint = new Layer_PositionSet_Point(startPointSet);
layerStartPoint.Active = true;
layerStartPoint.Point.IsDrawPointBorder = true;
layerStartPoint.Point.PointRadius = 5;
layerStartPoint.Point.PointColor = Color.PaleGreen;
layers.Add(layerStartPoint);
layerStartPoint.SpringLayerRepresentationChangedEvent(layerStartPoint);
IPosition_Connected end = (IPosition_Connected)layersEditedControl.GetMouseDoubleChickedNearestPositionInCurrentPositionSet(positionSet_Connected);
PositionSet_ConnectedEdit endPointSet = new PositionSet_ConnectedEdit();
endPointSet.AddPosition_Connected(end);
Layer_PositionSet_Point layerEndPoint = new Layer_PositionSet_Point(endPointSet);
layerEndPoint.Active = true;
layerEndPoint.Point.IsDrawPointBorder = true;
layerEndPoint.Point.PointRadius = 5;
layerEndPoint.Point.PointColor = Color.Cyan;
layers.Add(layerEndPoint);
layerEndPoint.SpringLayerRepresentationChangedEvent(layerEndPoint);
CountTimeTool.TimeCounter timeCounter = new CountTimeTool.TimeCounter();
searchPathEngine2.InitEngineForMap(positionSet_Connected);
double time2 = timeCounter.CountTimeForRepeatableMethod(delegate { searchPathEngine2.SearchPath(start, end); });
path = searchPathEngine2.SearchPath(start, end);
float pathLength2 = ((Dijkstra)searchPathEngine2).GetPathLength();
Console.WriteLine("Dijkstra consume time: " + time2 + " path Length = " + pathLength2);
searchPathEngine1.InitEngineForMap(positionSet_Connected);
//清空timeStampList以记录寻径过程各层的timeStamp
double time1 = timeCounter.CountTimeForRepeatableMethod(delegate { searchPathEngine1.SearchPath(start, end); });
timeStampList.Clear();
m2mPath = searchPathEngine1.SearchPath(start, end);
float pathLength1 = ((M2M_PF)searchPathEngine1).GetPathLength();
Console.WriteLine("M2M_PF consume time: " + time1 + " path Length = " + pathLength1);
//searchPathEngine1.SearchPath(start, end);
if (searchPathEngine1 is M2M_PF && m2mStructure != null)
{
for (int levelSequence = 1; levelSequence < m2mStructure.GetLevelNum(); levelSequence++)
{
ILevel level = m2mStructure.GetLevel(levelSequence);
IPart rootPart = m2mStructure.GetLevel(0).GetPartRefByPartIndex(0, 0);
IPositionSet positionSet = m2mStructure.GetDescendentPositionSetByAncestorPart(levelSequence, rootPart, 0);
List <IPosition_Connected> position_ConnectedList = new List <IPosition_Connected>();
positionSet.InitToTraverseSet();
while (positionSet.NextPosition())
{
if (positionSet.GetPosition() is IPart_Multi)
{
IPart_Multi partMulti = (IPart_Multi)positionSet.GetPosition();
IEnumerable <IPart_Connected> part_ConnectedEnumerable = partMulti.GetSubPartSet();
foreach (IPart_Connected part in part_ConnectedEnumerable)
{
IPosition_Connected tempPartConnected = (IPosition_Connected)part;
if (((Tag_M2M_Part)tempPartConnected.GetAttachment()).isNeedToSearch == true)
{
if (timeStampList[levelSequence - 1] == ((Tag_M2M_Part)tempPartConnected.GetAttachment()).timeStamp)
{
position_ConnectedList.Add(tempPartConnected);
}
}
}
}
else
{
IPosition_Connected tempPartConnected = (IPosition_Connected)positionSet.GetPosition();
if (((Tag_M2M_Part)tempPartConnected.GetAttachment()).isNeedToSearch == true)
{
if (timeStampList[levelSequence - 1] == ((Tag_M2M_Part)tempPartConnected.GetAttachment()).timeStamp)
{
position_ConnectedList.Add(tempPartConnected);
}
}
}
}
IPositionSet partSet = new PositionSet_Connected(position_ConnectedList);
Layer_M2MPartSetInSpecificLevel layer_M2MPartSetInSpecificLevel = new Layer_M2MPartSetInSpecificLevel(m2mStructure.GetLevel(levelSequence), partSet);
layer_M2MPartSetInSpecificLevel.Active = true;
layer_M2MPartSetInSpecificLevel.Alpha = 100;
//layer_M2MPartSetInSpecificLevel.
layers.Add(layer_M2MPartSetInSpecificLevel);
//PositionSet_Connected partSet_Connected = new PositionSet_Connected(position_ConnectedList);
//Layer_PositionSet_Connected layer_PartSet_Connected = new Layer_PositionSet_Connected(partSet_Connected);
////layer_PartSet_Connected.MainColor
////layer_PartSet_Connected.Active = true;
//layer_PartSet_Connected.SetPositionSetTransformByM2MLevel(level);
//layers.Add(layer_PartSet_Connected);
}
}
if (path != null)
{
Layer_PositionSet_Path layer = new Layer_PositionSet_Path(new PositionSet_Connected(path));
layer.Active = true;
layer.PathLine.LineColor = Color.Black;
layer.PathLine.LineWidth = 2;
layer.PathPoint.PointRadius = 2;
layer.PathPoint.IsDrawPointBorder = true;
//layer.EditAble = true;
//layer.Point.PointColor = Color.Yellow;
//layer.Point.PointRadius = 2;
//layer.Point.IsDrawPointBorder = true;
layers.Add(layer);
layer.SpringLayerRepresentationChangedEvent(layer);
}
else
{
this.BeginInvoke(new dShow(MessageBox.Show), new object[] { "There is no path between two node" });
}
if (m2mPath != null)
{
Layer_PositionSet_Path layer = new Layer_PositionSet_Path(new PositionSet_Connected(m2mPath));
layer.Active = true;
layer.PathLine.LineColor = Color.Red;
layer.PathLine.LineWidth = 2;
layer.PathPoint.PointRadius = 2;
layer.PathPoint.IsDrawPointBorder = true;
//layer.EditAble = true;
//layer.Point.PointColor = Color.Yellow;
//layer.Point.PointRadius = 2;
//layer.Point.IsDrawPointBorder = true;
layers.Add(layer);
layer.SpringLayerRepresentationChangedEvent(layer);
}
else
{
this.BeginInvoke(new dShow(MessageBox.Show), new object[] { "There is no path between two node by M2M_PF" });
}
}
}).BeginInvoke(null, null);
}
//没有路径则返回null
public List <IPosition_Connected> SearchPath(IPosition_Connected start, IPosition_Connected end)
{
List <IPosition_Connected> path = new List <IPosition_Connected>();
if (start == end)
{
path.Add(start);
return(path);
}
//初始化
if (!Init(start, end))
{
return(null);
}
PriorityQueue_List <IPosition_Connected> open_once = new PriorityQueue_List <IPosition_Connected>(open);
IPosition_Connected p = null, p_adj;
IPositionSet_Connected_Adjacency adj;
Tag p_tag, p_adj_tag;
float newDistance;
//当open表非空时
while (open_once.getSize() > 0)
{
p = open_once.getFirst();
if (p == end)
{
break;
}
open_once.removeFirst();
dict.TryGetValue(p, out p_tag);
adj = p.GetAdjacencyPositionSet();
adj.InitToTraverseSet();
while (adj.NextPosition())
{
p_adj = adj.GetPosition_Connected();
dict.TryGetValue(p_adj, out p_adj_tag);
newDistance = p_tag.g + adj.GetDistanceToAdjacency();
if (p_adj_tag.g > newDistance)
{
p_adj_tag.g = newDistance;
p_adj_tag.parent = p;
open_once.remove(p_adj);
open_once.add(p_adj);
}
}
}
//从终点根据标签中记录的父节点找到起点，生成路径
if (p == end)
{
path.Add(end);
p_tag = (Tag)end.GetAttachment();
while (p_tag.parent != null)
{
path.Add(p_tag.parent);
p_tag = (Tag)p_tag.parent.GetAttachment();
}
path.Reverse();
return(path);
}
else
{
return(null);
}
}
//没有路径则返回null
public List <IPosition_Connected> SearchPath(IPosition_Connected start, IPosition_Connected end)
{
List <IPosition_Connected> path = new List <IPosition_Connected>();
if (start == end)
{
path.Add(start);
return(path);
}
//初始化
if (!Init(start, end))
{
return(null);
}
IPosition_Connected p = null, p_adj;
IPositionSet_Connected_Adjacency adj_set;
Tag p_tag, p_adj_tag;
float newF, newG, dx, dy;
IPriorityQueue <IPosition_Connected> open;
int num = positionSet.GetNum();
if (num > 0)
{
open = new PriorityQueue <IPosition_Connected>(num, com);
}
else
{
open = new PriorityQueue <IPosition_Connected>(com);
}
//int count = 0;
//if (debug)
// Console.WriteLine("SearchPath:");
//将起点加入open表
open.add(start);
//当open表非空时
while (open.getSize() > 0)
{
//获得下一个最近的点
p = open.removeFirst();
//到达终点则结束
if (p == end)
{
break;
}
p_tag = (Tag)p.GetAttachment();
p_tag.closed = true;
adj_set = p.GetAdjacencyPositionSet();
adj_set.InitToTraverseSet();
//if (debug)
//{
// count++;
// Console.Write(p.ToString() + "\t");
//}
while (adj_set.NextPosition())
{
p_adj = adj_set.GetPosition_Connected();
p_adj_tag = (Tag)p_adj.GetAttachment();
if (p_adj_tag.timeStamp != time_stamp)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
newF = newG + evaluate(p_adj.GetX(), p_adj.GetY(), end.GetX(), end.GetY());
p_adj_tag.parent = p;
p_adj_tag.g = newG;
p_adj_tag.f = newF;
p_adj_tag.closed = false;
p_adj_tag.timeStamp = time_stamp;
open.add(p_adj);
}
else
{
if (!p_adj_tag.closed)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
dx = end.GetX() - p_adj.GetX();
dy = end.GetY() - p_adj.GetY();
newF = newG + evaluate(p_adj.GetX(), p_adj.GetY(), end.GetX(), end.GetY());
if (newF < p_adj_tag.f)
{
p_adj_tag.parent = p;
p_adj_tag.g = newG;
p_adj_tag.f = newF;
open.update(p_adj);
}
}
}
//if (debug)
// Console.Write(p_adj.ToString() + ":" + p_adj.GetAttachment().ToString() + "\t");
}
//if (debug)
// Console.WriteLine();
}
//if (debug)
//{
// Console.Write("position count:");
// Console.WriteLine(count);
//}
//从终点根据标签中记录的父节点找到起点，生成路径
if (p == end)
{
path.Add(end);
p_tag = (Tag)end.GetAttachment();
while (p_tag.parent != null)
{
path.Add(p_tag.parent);
p_tag = (Tag)p_tag.parent.GetAttachment();
}
path.Reverse();
return(path);
}
else
{
return(null);
}
}