Example #1
0
    public bool FindPath(ANode root, ANode dest)
    {
        PQueue queue = new PQueue();

        if (!nodes.Contains(root))
        {
            nodes.Add(root);
            foreach (APath x in root.GetPaths())
            {
                paths.Add(x);
            }
        }

        if (!nodes.Contains(dest))
        {
            nodes.Add(dest);
            foreach (APath x in dest.GetPaths())
            {
                paths.Add(x);
            }
        }

        ANode currentNode = root;

        currentNode.Cost = 0;

        List <ANode> notVisited = new List <ANode>(nodes);

        while (notVisited.Count > 0)
        {
            foreach (APath x in currentNode.GetPaths())
            {
                ANode neighbor = x.GetOtherSide(currentNode);
                if (neighbor.Visited)
                {
                    continue;
                }

                neighbor.UpdateHeuristic(GetHeuristicOf(neighbor, dest));

                if (neighbor.Cost > currentNode.Cost + x.Cost)
                {
                    neighbor.UpdateNodeCost(currentNode.Cost + x.Cost, currentNode);
                }

                queue.Enqueue(neighbor);
            }
            currentNode.Visited = true;
            notVisited.Remove(currentNode);
            if (notVisited.Count == 0)
            {
                break;
            }
            currentNode = queue.Dequeue();
            if (currentNode.Equals(dest))
            {
                UpdatePath(root, dest);
                return(true);
            }
        }

        return(false);
    }