A*算法，显示路径

 

private void DrawPath(long nStart, long nEnd)
    {
        Cursor.Current = Cursors.WaitCursor;
        if (nStart < 1)
            nStart = 1;
        else if (nStart > mDataList.Num)
            nStart = mDataList.Num;
        if (nEnd < 1)
            nEnd = 1;
        else if (nEnd > mDataList.Num)　
            nEnd = mDataList.Num;
        TNodeLink startLink = new TNodeLink();
        startLink.G = 0;
        startLink.Current = mDataList[nStart];

        TNodeLink endLink = new TNodeLink();
        endLink.H = 0;
        endLink.Current = mDataList[nEnd];
            
        //距离估价函数（合理性？）
        startLink.H = THelp.Dist(startLink.Current.Sx, endLink.Current.Sx, startLink.Current.Sy, endLink.Current.Sy);
        endLink.G = startLink.H;

        //估价函数F(n)=G(n)+H(n)
        startLink.F = startLink.G + startLink.H;
        endLink.F = endLink.G + endLink.H;

        OPEN  = new TNodeColl();
        CLOSE = new TNodeColl();
        OPEN.AddNode(startLink);

        TNodeLink parent,tmp;

        //设置坐标系NAD83
        MapInfo.Geometry.CoordSys coordSys = MapInfo.Engine.Session.Current.CoordSysFactory.CreateLongLat(DatumID.NAD83);

        while (!OPEN.IsNull())
        {
            tmp = OPEN.FetchBest();//寻找最近节点链表
            parent = tmp;
            OPEN.RemoveNode(tmp);
            if(tmp.Current == endLink.Current)//若最佳路线已经找到
            {
                    
                由CLOSE直接添加线段#region 由CLOSE直接添加线段
                TNodeLink tmpLink=CLOSE.Foot;

                //创建表
                MapInfo.Data.TableInfoMemTable ti=new MapInfo.Data.TableInfoMemTable("pathLine");
                ti.Columns.Add(MapInfo.Data.ColumnFactory.CreateFeatureGeometryColumn(coordSys));
                ti.Columns.Add(MapInfo.Data.ColumnFactory.CreateStyleColumn());
                MapInfo.Data.Table  table = MapInfo.Engine.Session.Current.Catalog.CreateTable(ti);
            
            
                //线段创建条数跟踪测试
                int segmentTest=1;

                //计算距离
                double tmpDistance;

                MapInfo.Styles.LineWidth lineWidth = new MapInfo.Styles.LineWidth(3,MapInfo.Styles.LineWidthUnit.Pixel);
                MapInfo.Styles.SimpleLineStyle simpleLineStyle = new MapInfo.Styles.SimpleLineStyle (lineWidth,2,System.Drawing.Color.Red);



                while(null!=CLOSE.Fetch(tmpLink.Current.Parent))
                {
                    //确定线段
                    MapInfo.Geometry.DPoint startDPoint,endDPoint;
                    startDPoint.x=tmpLink.Current.Sx;
                    startDPoint.y=tmpLink.Current.Sy;
                    endDPoint.x=CLOSE.Fetch(tmpLink.Current.Parent).Current.Sx;
                    endDPoint.y=CLOSE.Fetch(tmpLink.Current.Parent).Current.Sy;

                    //旅行距离计算
                    tmpDistance=111*Math.Sqrt((startDPoint.x-endDPoint.x)*(startDPoint.x-endDPoint.x)+(startDPoint.y-endDPoint.y)*(startDPoint.y-endDPoint.y));
                

                    //创建线段
                    MapInfo.Geometry.MultiCurve line=MapInfo.Geometry.MultiCurve.CreateLine(coordSys,startDPoint,endDPoint);

                    //添加特征
                    MapInfo.Data.Feature f=new Feature(line,simpleLineStyle);//单独设置当前feature的样式的方法
                    table.InsertFeature(f);

                    tmpLink=CLOSE.Fetch(tmpLink.Current.Parent);
                        

                    //计算总旅行距离
                    distance+=tmpDistance;


                    //线段创建条数跟踪测试
                    segmentTest+=1;
                }

                //添加到图层并显示
                MapInfo.Mapping.IMapLayer layer=new MapInfo.Mapping.FeatureLayer(table);
                MapControl1.Map.Layers.Insert(2,layer);

                #endregion

            }
            TNodeLink child = null;
            parent = tmp;
            for (int i=0; i<tmp.Current.NodeNum; i++)
            {
                if (OPEN.ExistNode(parent.Current[i]))
                {
                    child = OPEN.Fetch(parent.Current[i]);
                    double mf, mg, mh;
                    mf = mg = mh = 0;

                    //计算实际代价
                    mg = parent.G + THelp.Dist(parent.Current.Sx,parent.Current[i].Sx,parent.Current.Sy,parent.Current[i].Sy);

                    //计算估计代价，距离算法
                    mh = THelp.Dist(parent.Current[i].Sx,endLink.Current.Sx,parent.Current[i].Sy, endLink.Current.Sy);

                    //估价函数
                    mf = mg + mh;
                    if (mf < child.F)
                    {
                        child.G = mg;
                        child.H = mh;
                        child.F = mf;
                        child.Current.Parent = parent.Current;
                    }
                }
                else if (CLOSE.ExistNode(parent.Current[i]))
                {
                }
                else
                {
                    child = new TNodeLink();
                    child.Current = parent.Current[i];
                    child.Current.Parent = parent.Current;
                    child.G = parent.G + THelp.Dist(parent.Current.Sx,parent.Current[i].Sx,parent.Current.Sy,parent.Current[i].Sy);
                    child.H = THelp.Dist(parent.Current[i].Sx, endLink.Current.Sx,parent.Current[i].Sy, endLink.Current.Sy);
                    child.F = child.G + child.H;
                    OPEN.AddNode(child);
                }
            }
            CLOSE.AddNode(parent);
            if (tmp.Current == endLink.Current)
                CLOSE.AddNode(tmp);

        }
        CLOSE.PrintPath();//测试树，待删！
            
        //计算旅行时间
        time=distance/40.0;
            
        //地图更新
        TextBox7.Text=distance.ToString("0.00");
        TextBox8.Text=time.ToString("0.00");
        MapControl1.Map.Invalidate();

        //恢复鼠标状态
        Cursor.Current=Cursors.Default;
    }