Example #1
0
        public static bool Search <T>(IAstarGraph <T> graph, T start, T goal, out Dictionary <T, T> cameFrom)
        {
            var foundPath = false;

            cameFrom = new Dictionary <T, T> {
                { start, start }
            };

            var costSoFar = new Dictionary <T, int>();
            var frontier  = new List <Tuple <int, T> > {
                new Tuple <int, T>(0, start)
            };

            costSoFar[start] = 0;

            while (frontier.Count > 0)
            {
                var current = frontier[0];
                frontier.RemoveAt(0);

                if (current.Item2.Equals(goal))
                {
                    foundPath = true;
                    break;
                }

                foreach (var next in graph.GetNeighbors(current.Item2))
                {
                    var newCost = costSoFar[current.Item2] + graph.Cost(current.Item2, next);
                    if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
                    {
                        costSoFar[next] = newCost;
                        var priority = newCost + graph.Heuristic(next, goal);
                        frontier.Add(new Tuple <int, T>(priority, next));
                        cameFrom[next] = current.Item2;
                    }
                }

                frontier.Sort(new TupleComparer <T>());
            }

            return(foundPath);
        }
Example #2
0
        public static bool Search <T>(IAstarGraph <T> graph, T start, T goal, out Dictionary <T, T> cameFrom)
        {
            var foundPath = false;

            cameFrom = new Dictionary <T, T>();
            cameFrom.Add(start, start);

            var costSoFar = new Dictionary <T, int>();
            var frontier  = new PriorityQueue <AStarNode <T> >(1000);

            frontier.Enqueue(new AStarNode <T>(start), 0);

            costSoFar[start] = 0;

            while (frontier.Count > 0)
            {
                var current = frontier.Dequeue();

                if (current.Data.Equals(goal))
                {
                    foundPath = true;
                    break;
                }

                foreach (var next in graph.GetNeighbors(current.Data))
                {
                    var newCost = costSoFar[current.Data] + graph.Cost(current.Data, next);
                    if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
                    {
                        costSoFar[next] = newCost;
                        var priority = newCost + graph.Heuristic(next, goal);
                        frontier.Enqueue(new AStarNode <T>(next), priority);
                        cameFrom[next] = current.Data;
                    }
                }
            }

            return(foundPath);
        }