コード例 #1
0
        public List <T> FindPath(T start, T goal)
        {
            if (start == null)
            {
                return(null);
            }
            if (goal == null)
            {
                return(null);
            }
            this.Goal = goal;
            gScore    = new Dictionary <T, float>();
            fScore    = new Dictionary <T, float>();


            //var openset = new HashSet<T>();
            var openset   = new FibonacciQueue <T, float>(d => fScore[d]);
            var closedset = new HashSet <T>();

            cameFrom = new Dictionary <T, T>();

            //came_from := the empty map    // The map of navigated nodes.

            gScore[start] = 0;    // Cost from start along best known path.
            // Estimated total cost from start to goal through y.
            fScore[start] = gScore[start] + ConnectionProvider.GetHeuristicCostEstimate(start, goal);


            openset.Enqueue(start);
            onBegin();
            while (openset.Count > 0)
            {
                var current = openset.Dequeue();

                onSelectNode(current);

                if (StopCondition(current))
                {
                    return(reconstruct_reverse_path(cameFrom, current).Reverse().ToList());
                }

                if (current.Equals(goal))
                {
                    return(reconstruct_reverse_path(cameFrom, goal).Reverse().ToList());
                }

                closedset.Add(current); //add current to closedset

                foreach (var neighbor in ConnectionProvider.GetConnectedNodes(this, current))
                {
                    if (closedset.Contains(neighbor))
                    {
                        continue;
                    }

                    var tentative_g_score = gScore[current] + ConnectionProvider.GetCost(current, neighbor);
                    if (openset.Contains(neighbor) && !(tentative_g_score <= gScore[neighbor]))
                    {
                        continue;
                    }

                    cameFrom[neighbor] = current;
                    gScore[neighbor]   = tentative_g_score;
                    fScore[neighbor]   = gScore[neighbor] + ConnectionProvider.GetHeuristicCostEstimate(neighbor, goal);

                    if (openset.Contains(neighbor))
                    {
                        continue;
                    }

                    openset.Enqueue(neighbor);
                    onEnqueueNode(neighbor);
                }
            }
            return(null);
        }