astarpathfinder.cs

实现A*算法的C#源代码

/*
    作者：Tom Xu([e-mail])
    创建时间：2006-11-03 13:40
    内容描述：生成邻接矩阵的所需要的数据结构
*/    

using System;
using System.Collections.Generic;
using System.Text;

namespace Iaspec.GIS.Common
{
    public class AStarPathFinder
    {
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="anList">邻接结点列表</param>
        /// <param name="adList">邻接结点间距离列表</param>
        /// <param name="srcNode">起始结点</param>
        /// <param name="desNode">目标结点</param>
        public AStarPathFinder(List<AdjoinNode> anList, List<AdjoinDistance> adList,AdjoinNode srcNode,AdjoinNode desNode)
        {
            _adjoinNodeList = anList;
            _adjoinDisList = adList;
            _srcNode = srcNode;
            _desNode = desNode;
            _solutionPathList = new List<Node>();

            if (_adjoinDisList.Count > 1 && _adjoinNodeList.Count > 1 && _srcNode != null && _desNode != null)
            {
                Initialize();
            }
            else
            {
                throw new CantAnalysisException("分析所需的条件未完全准备好!");
            }
            
        }
        
        private List<AdjoinNode> _adjoinNodeList;   //邻接结点列表
        private List<AdjoinDistance> _adjoinDisList;    //邻接结点间距离列表
        private System.Nullable<AdjoinNode> _srcNode;                //起始结点
        private System.Nullable<AdjoinNode> _desNode;                //目标结点
        private int _srcID;                         //起始结点ID
        private int _desID;                         //目标结点ID
        private List<Node> _solutionPathList;
        /// <summary>
        /// 结果路径
        /// </summary>
        public List<Node> SolutionPathList
        {
            get { return _solutionPathList; }
        }

        public double TotalCost
        {
            get
            {
                if (_solutionPathList.Count == 0 || _solutionPathList.Count == 1 || _map.Matrics == null)
                {
                    return 0;
                }
                else
                {
                    double dis = 0;
                    for (int i = 0; i < _solutionPathList.Count - 1; i++)
                    {
                        Node node1 = _solutionPathList[i];
                        Node node2 = _solutionPathList[i + 1];
                        dis += _map.Matrics[node1.ID, node2.ID];
                    }
                    return dis;
                }
            }
        }

            private Map _map;

        /// <summary>
        /// 初始化邻接矩阵
        /// </summary>
        private void Initialize()
        {
            _map = new Map();
            
            int nodeCount = _adjoinNodeList.Count;
            double[,] Matrics = new double[nodeCount, nodeCount];
            for (int i = 0; i < nodeCount; i++)
            {
                for (int j = 0; j < nodeCount; j++)
                {
                    if (i != j) Matrics[i, j] = -1;
                }
            }

            _adjoinDisList.ForEach(delegate(AdjoinDistance ad)
            {
                Matrics[ad.ID1, ad.ID2] = ad.Length;
                Matrics[ad.ID2, ad.ID1] = ad.Length;
            });
            _map.Matrics = Matrics;            
        }

        /// <summary>
        /// 算法主体
        /// </summary>
        public void FindPath()
        {
            _srcID = _adjoinNodeList.IndexOf(_srcNode.Value);
            _desID = _adjoinNodeList.IndexOf(_desNode.Value);
            if (_srcID > 0 && _desID > 0 && _map.Matrics != null)
            {
                Node goalNode = new Node(null, null, 0, _desNode.Value.X, _desNode.Value.Y, _desID);
                Node startNode = new Node(null, goalNode, 0, _srcNode.Value.X, _srcNode.Value.Y, _srcID);

                //新建OPEN&CLOSED表
                SortedCostNodeList OPEN = new SortedCostNodeList();
                SortedCostNodeList CLOSED = new SortedCostNodeList();

                //将起始结点置入OPEN表中
                OPEN.Push(startNode);

                while (OPEN.Count > 0)
                {
                    //从OPEN表中取出cost最小的结点
                    Node currentNode = OPEN.Pop();
                    
                    if (currentNode.Equals(goalNode))
                    {
                        goalNode.ParentNode = currentNode.ParentNode;
                        break;
                    }

                    //获取与当前结点相连的结点
                    List<Node> successors = GetSuccessors(_adjoinNodeList, currentNode, goalNode);

                    foreach (Node node_successor in successors)
                    {                        
                        int oFound = OPEN.IndexOf(node_successor);

                        if (oFound > 0)
                        {
                            Node existing_node = OPEN.NodeAt(oFound);
                            if (existing_node.CompareTo(currentNode) <= 0)
                                continue;
                        }

                        int cFound = CLOSED.IndexOf(node_successor);

                         if (cFound > 0)
                        {
                            Node existing_node = CLOSED.NodeAt(cFound);
                            if (existing_node.CompareTo(currentNode) <= 0)
                                continue;
                        }

                        if (oFound != -1)
                            OPEN.RemoveAt(oFound);
                        if (cFound != -1)
                            CLOSED.RemoveAt(cFound);

                        OPEN.Push(node_successor);
                    }
                    CLOSED.Push(currentNode);
                }


                //根据parentNode属性沿目标结点到起始结点方向获得路径
                Node node = goalNode;
                while (node != null)
                {
                    _solutionPathList.Insert(0, node);
                    node = node.ParentNode;
                }                
                

            }
        }
                
        private List<Node> GetSuccessors(List<AdjoinNode> adjoinNodeList, Node currentNode, Node goalNode)
        {
            List<Node> successors = new List<Node>();
            for (int i = 0; i < _map.Matrics.GetUpperBound(1); i++)
            {
                if (_map.Matrics[currentNode.ID, i] > 0)
                {
                    Node node = new Node(currentNode, goalNode, currentNode.TotalCost + _map.Matrics[currentNode.ID, i], adjoinNodeList[i].X, adjoinNodeList[i].Y, i);
                    if (!node.Equals(currentNode) && !node.Equals(currentNode.ParentNode))
                        successors.Add(node);
                }
            }
            return successors;
        }
        
    }
    
    [global::System.Serializable]
    public class CantAnalysisException : Exception
    {  
        public CantAnalysisException() { }
        public CantAnalysisException(string message) : base(message) 
        {

        }
        public CantAnalysisException(string message, Exception inner) : base(message, inner) { }
        protected CantAnalysisException(
          System.Runtime.Serialization.SerializationInfo info,
          System.Runtime.Serialization.StreamingContext context)
            : base(info, context) { }
    }
}