public static IDictionary <TNode, int> GetCosts <TNode>(TNode toStartFrom,
                                                                IWeighedGraph <TNode> map, IEqualityComparer <TNode> nodeComparer)
        {
            IPriorityQueue <int, TNode> open   = new SkipListPriorityQueue <int, TNode>(10);
            HashSet <TNode>             closed = new HashSet <TNode>(nodeComparer);
            IDictionary <TNode, int>    costs  = new Dictionary <TNode, int>(nodeComparer);

            costs[toStartFrom] = 0;
            open.Enqueue(toStartFrom, 0);

            while (open.Count > 0)
            {
                int   cost;
                TNode node = open.Dequeue(out cost);
                closed.Add(node);

                foreach (TNode neighbour in map.GetNeighbours(node))
                {
                    int newCost = cost + map.GetMovementCost(node, neighbour);
                    int oldCost;
                    if (!costs.TryGetValue(neighbour, out oldCost))
                    {
                        open.Enqueue(neighbour, newCost);
                        costs[neighbour] = newCost;
                    }
                    else if (newCost < oldCost)
                    {
                        open.Remove(neighbour, oldCost);
                        open.Enqueue(neighbour, newCost);
                        costs[neighbour] = newCost;
                    }//http://theory.stanford.edu/~amitp/GameProgramming/ImplementationNotes.html
                }
            }
            return(costs);
        }
        public static List <TNode> GetArea <TNode>(TNode toStartFrom, IWeighedGraph <TNode> map,
                                                   IEqualityComparer <TNode> nodeComparer, int movementLimit)
        {
            IPriorityQueue <int, TNode> open   = new SkipListPriorityQueue <int, TNode>(10);
            HashSet <TNode>             closed = new HashSet <TNode>(nodeComparer);
            ICostCollection <TNode>     costs  = map.GetTempCostCollection();

            costs[toStartFrom] = 0;
            open.Enqueue(toStartFrom, 0);

            while (open.Count > 0)
            {
                int   cost;
                TNode node = open.Dequeue(out cost);
                closed.Add(node);

                if (cost < movementLimit)
                {
                    foreach (TNode neighbour in map.GetNeighbours(node))
                    {
                        int newCost = cost + map.GetMovementCost(node, neighbour);
                        int oldCost;
                        if (!costs.TryGetValue(neighbour, out oldCost))
                        {
                            open.Enqueue(neighbour, newCost);
                            costs[neighbour] = newCost;
                        }
                        else if (newCost < oldCost)
                        {
                            open.Remove(neighbour, oldCost);
                            open.Enqueue(neighbour, newCost);
                            costs[neighbour] = newCost;
                        }
                    }
                }
            }

            List <TNode> result = new List <TNode>(closed.Count);

            result.AddRange(closed.Where(node => costs[node] < movementLimit));
            costs.Release();
            return(result);
        }
Exemple #3
0
        public static List <TNode> GetPath <TNode>(TNode start, TNode goal,
                                                   IWeighedGraph <TNode> map, IHeurestic <TNode> heurestics, IEqualityComparer <TNode> nodeComparer)
        {
            IPriorityQueue <int, TNode> open =
                new SkipListPriorityQueue <int, TNode>(10);
            HashSet <TNode>            closed  = new HashSet <TNode>(nodeComparer);
            ICostCollection <TNode>    gCosts  = map.GetTempCostCollection();
            ICostCollection <TNode>    hCosts  = map.GetTempCostCollection();
            IDictionary <TNode, TNode> parents = new Dictionary <TNode, TNode>(nodeComparer);

            open.Enqueue(start, 0);
            gCosts[start] = 0;
            hCosts[start] = 0;
            while (open.Count > 0 && !nodeComparer.Equals(open.Peek(), goal))
            {
                TNode current = open.Dequeue();
                closed.Add(current);

                foreach (TNode neighbour in map.GetNeighbours(current))
                {
                    int gCost = gCosts[current] + map.GetMovementCost(current, neighbour);
                    int oldGcost;
                    if (gCosts.TryGetValue(neighbour, out oldGcost) && gCost < oldGcost)
                    {//If we found a better route to neighbour
                        int hCost = hCosts[neighbour];
                        open.Remove(neighbour, oldGcost + hCost);
                        closed.Remove(neighbour);

                        gCosts[neighbour] = gCost;
                        open.Enqueue(neighbour, gCost + hCost);
                        parents[neighbour] = current;
                    }
                    else if (!closed.Contains(neighbour) && !open.Contains(neighbour))
                    {//If we got here the first time
                        int hCost = heurestics.GetCostEstimate(neighbour);
                        hCosts[neighbour] = hCost;

                        gCosts[neighbour] = gCost;
                        open.Enqueue(neighbour, gCost + hCost);
                        parents[neighbour] = current;
                    }
                }
            }

            gCosts.Release();
            hCosts.Release();

            if (open.Count == 0)//No path exists
            {
                return(new List <TNode>());
            }

            TNode last = open.Dequeue();

            open.Clear();

            List <TNode> result = new List <TNode>();

            while (parents.ContainsKey(last))
            {
                result.Add(last);
                last = parents[last];
            }

            result.Add(last);
            result.Reverse();
            return(result);
        }