//每次寻径之前根据起点、终点信息做初始化 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); } }