예제 #1
0
        public static List <int> AStar <T>(ioGraph <T> _graph, int _start, int _end, Func <int, double> _fHeuristic)
        {
            //var startRecord = new NodeRecord(_start, -1, 0, _fHeuristic(_start));
            var startRecord = new NodeRecord <T>(_start, null, 0, _fHeuristic(_start));

            var openList = new Dictionary <int, NodeRecord <T> >();

            openList.Add(_start, startRecord);
            var closedList = new Dictionary <int, NodeRecord <T> >();
            int curNode    = -1;

            while (openList.Count > 0)
            {
                //Find smallest element in open list (est total cost)
                curNode = FindSmallest(openList);

                //Exit if goal found
                if (curNode == _end)
                {
                    closedList.Add(curNode, openList[curNode]);
                    break;
                }

                var curEdges = _graph.GetConnectionsFast(curNode);

                foreach (ioGraph <T> .ioGraphEdge edge in curEdges)
                {
                    //Set end node and add cost
                    var            endNode     = edge.ToNode;
                    var            endNodeCost = openList[curNode].CostSoFar + edge.Weight;
                    NodeRecord <T> endNodeRecord;
                    double         endNodeHeuristic;

                    if (openList.ContainsKey(endNode))
                    {
                        if (openList[endNode].CostSoFar <= endNodeCost)
                        {
                            continue;
                        }
                        openList.Remove(endNode);
                    }
                    if (closedList.ContainsKey(endNode))
                    {
                        if (closedList[endNode].CostSoFar <= endNodeCost)
                        {
                            continue;
                        }
                        closedList.Remove(endNode);
                    }

                    endNodeRecord          = new NodeRecord <T>();
                    endNodeRecord.FromNode = curNode;
                    endNodeHeuristic       = _fHeuristic(endNode);

                    endNodeRecord.CostSoFar    = endNodeCost;
                    endNodeRecord.Connection   = edge;
                    endNodeRecord.EstTotalCost = endNodeCost + endNodeHeuristic;
                    openList.Add(endNode, endNodeRecord);


                    /*if(closedList.ContainsKey(endNode))
                     * {
                     *
                     *  if (closedList[endNode].CostSoFar <= endNodeCost)
                     *      continue;
                     *
                     *  endNodeRecord = closedList[endNode];
                     *  endNodeHeuristic = endNodeRecord.EstTotalCost - endNodeRecord.CostSoFar;
                     *  closedList.Remove(endNode);
                     * }
                     * if(openList.ContainsKey(endNode))
                     * {
                     *  if (openList[endNode].CostSoFar <= endNodeCost)
                     *      continue;
                     *
                     *  endNodeRecord = openList[endNode];
                     *  endNodeHeuristic = endNodeRecord.EstTotalCost - openList[endNode].CostSoFar;
                     *  openList.Remove(endNode);
                     *
                     * }
                     * else
                     * {
                     *  endNodeRecord = new NodeRecord();
                     *  endNodeRecord.FromNode = curNode;
                     *  endNodeHeuristic = _fHeuristic(endNode);
                     * }
                     *
                     * endNodeRecord.CostSoFar = endNodeCost;
                     * endNodeRecord.Connection = edge;
                     * endNodeRecord.EstTotalCost = endNodeCost + endNodeHeuristic;
                     *
                     * if (!openList.ContainsKey(endNode)) openList.Add(endNode, endNodeRecord);*/
                }

                closedList.Add(curNode, openList[curNode]);
                openList.Remove(curNode);
            }
            if (curNode != _end)
            {
                String dbgMsg = "End Node Null -- No path found?";
                Msg.LogDebug(TAG_DEBUG, dbgMsg, MsgPriLvl.HIGH);
                return(null);
            }
            else
            {
                var path = new List <int>();
                while (curNode != _start)
                {
                    path.Add(curNode);
                    Msg.LogDebug(TAG_DEBUG, "Added to path: " + _graph.GetNode(curNode), MsgPriLvl.LOW);
                    curNode = closedList[curNode].FromNode;  //Seed 705 crashes here with invalid key
                }

                return(path);
            }
        }