AStar算法

使用A*算法 计算路径, 算法测试中使用excel文件提供数据, 算法完成后,在excel中显示找到的路径

主要代码:

  /// <summary>
        /// 使用A* 算法
        /// </summary>
        /// <param name="graph"> 整个立库节点信息  </param>
        /// <param name="startNode">开始的节点编号</param>
        /// <param name="endNode">目标点的节点编号</param>  
        /// <returns>是否有用的路径</returns>
        public bool SearchPath(List<IPathNode> graph, IPathNode startNode, IPathNode endNode)
        {
            init(graph);

            APathNode startPathNode = GetAPathNode(startNode);
            APathNode endPathNode = GetAPathNode(endNode);

            if (startPathNode == null || endPathNode == null)
            { //节点不存在
                return false;
            }

            try
            {
                //将起点加入开表
                openList.Add(startPathNode);
                while (openList.Count > 0)
                {
                    APathNode currentNode = openList.Dequeue();//取出第一元素,在开表中删除

                    //将当前的节点移除开表，放入闭表中（将当前的节点标记为已评估节点）  
                    closeList.Add(currentNode); 

                     //如果评估点等于终点，找到寻路路径 ,结束寻路
                    if (currentNode == endPathNode)
                    {
                        RetracePath(endPathNode);
                        return true;
                    }
                    else
                    {
                        //遍历评估节点 
                        List<APathNode> neighborNodes = GetNeighborNodes(currentNode);
                        foreach (APathNode neiNode in neighborNodes)
                        {
                            //如果闭表已经有该点（该点已经被评估过）或者该点是障碍物
                            if (!neiNode.NodeObj.IsAvailable || closeList.Contains(neiNode))
                            {
                                continue;
                            }

                            //新的 起点->neighbor =当前点->邻近点 +当前点->起点
                            //计算 该邻近点到起点总花费=当前到邻近点的距离+当前点到起点的花费  
                            float newGGost = GetGCost(currentNode, neiNode);

                            //1.如果邻近点到起点总花费<之前已计算的花费，说明之前的路径存在绕远路，将邻近点的上一节点改为当前点
                            //  因为计算过邻近点Gcost，所有一定存在开表中，后面值为false
                            //2》.开表中不包含邻近点，该节点没有被遍历
                            bool flagContain = openList.Contains(neiNode); //是否包含相邻节点
                            if (newGGost < neiNode.GCost || !flagContain)
                            {
                                //更新邻近点的三大代价，将邻近点的上一节点设置为当前节点
                                neiNode.GCost = newGGost;
                                neiNode.HCost = GetHCost(neiNode, endPathNode);
                                neiNode.FCost = GetFCost(neiNode);
                                neiNode.PNode = currentNode;

                                //2》.如果开表中不包含邻近点，该节点没有被遍历
                                if (!flagContain)
                                {
                                    openList.Add(neiNode);
                                }
                            } //end if 
                        } // end foreach 
                    }
                } //end  while
            }
            finally
            {
                //清除过程数据
                closeList.Clear();
                openList.Clear();
                mapNodes.Clear();
            }

            return false;
        }

 

参考:【A星算法】A星寻路算法详解（小白也可以看懂+C#代码+零基础学习A*）-CSDN博客