Ejemplo n.º 1
0
def UCSRoute(graph, startVert, goalVert):

    if startVert == goalVert:
        return []

    minHeap = PriorityQueue()
    minHeap.insert(0, startVert)
    pred_cost = {}

    visited = set()
    pred = {}
    while not minHeap.isEmpty():
        nextVert = minHeap.firstElement()  #nextVert = [cost, vert]
        minHeap.delete()
        print("--------------")
        print("Popping", nextVert)
        if nextVert[1] in visited:
            continue
        else:
            visited.add(nextVert[1])
            if (nextVert[0], nextVert[1]) in pred_cost.keys():
                pred[nextVert[1]] = pred_cost[(nextVert[0], nextVert[1])]
            else:
                pred[nextVert[1]] = None
            if goalVert == nextVert[1]:
                return reconstructPath(startVert, goalVert, pred)
        neighbors = graph.getNeighbors(nextVert[1])
        for n in neighbors:
            neighNode = n[0]
            edgeCost = n[1]
            if neighNode not in visited:
                minHeap.insert(edgeCost + nextVert[0], neighNode)
                pred_cost[(edgeCost + nextVert[0], neighNode)] = nextVert[1]

    return "NO PATH"
Ejemplo n.º 2
0
def UCSRoute(graph, startVert, goalVert):
    """This algorithm search a graph using Uniform Cost Search algorithm"""
    if startVert == goalVert:
        return []
    q = PriorityQueue()
    q.insert(0, (startVert, None))
    visited = set()
    pred = {}
    while not q.isEmpty():
        weight, (nextVert, predNextvert) = q.firstElement()
        q.delete()
        if nextVert in visited:
            pass
        else:
            visited.add(nextVert)
            pred[nextVert] = predNextvert
            if nextVert == goalVert:
                print("UCS number of visited: ", len(visited))
                return reconstructPath(startVert, goalVert, pred)
            neighbors = graph.getNeighbors(nextVert)
            for n in neighbors:
                if type(n) != int:
                    # NOTICE: From getNeighbors, the order is (vert, weight)
                    weight_n = n[1]
                    n = n[0]
                if n not in visited:
                    q.insert(weight + weight_n, (n, nextVert))
    return "NO PATH"
Ejemplo n.º 3
0
def AStarRoute(graph, startVert, goalVert):
    """This algorithm search a graph using A star Search algorithm"""
    if startVert == goalVert:
        return []
    q = PriorityQueue()
    q.insert(0, (startVert, None))
    visited = set()
    pred = {}
    while not q.isEmpty():
        weight, (nextVert, predNextvert) = q.firstElement()
        # The weight in PriorityQueue also contains the heuristicDist, not the actual weight
        # Calculate the actual weight
        weight = weight - graph.heuristicDist(nextVert, goalVert)
        q.delete()
        if nextVert in visited:
            pass
        else:
            visited.add(nextVert)
            pred[nextVert] = predNextvert
            if nextVert == goalVert:
                print("Astar number of visited: ", len(visited))
                return reconstructPath(startVert, goalVert, pred)
            neighbors = graph.getNeighbors(nextVert)
            for n in neighbors:
                if type(n) != int:
                    # NOTICE: From getNeighbors, the order is (vert, weight)
                    weight_n = n[1]
                    n = n[0]
                if n not in visited:
                    q.insert(weight + weight_n + graph.heuristicDist(n, goalVert), (n, nextVert))
    return "NO PATH"
Ejemplo n.º 4
0
def dijkstras(graph, startVert, goalVert):
    """ This algorithm searches a graph using Dijkstras algorithm to find
    the shortest path from every point to a goal point (actually
    searches from goal to every point, but it's the same thing.
    It uses a priority queue to store the indices of vertices that it still
    needs to examine.
    It returns the best path frmo startVert to goalVert, but otherwise
    startVert does not play into the search."""
    num_q_nodes_removed = 0
    max_queue_size = -1
    if startVert == goalVert:
        return []
    q = PriorityQueue()
    visited = set()
    pred = {}
    cost = {}
    for vert in graph.getVertices():
        cost[vert] = 1000.0
        pred[vert] = None
        q.insert(cost[vert], vert)
    visited.add(goalVert)
    cost[goalVert] = 0
    q.update(cost[goalVert], goalVert)
    while not q.isEmpty():
        (nextCTG, nextVert) = q.firstElement()
        if q.getSize() > max_queue_size:
            max_queue_size = q.getSize()
        q.delete()
        num_q_nodes_removed += 1
        visited.add(nextVert)
        # print("--------------")
        # print("Popping", nextVert, nextCTG)
        neighbors = graph.getNeighbors(nextVert)
        for n in neighbors:
            neighNode = n[0]
            edgeCost = n[1]
            if neighNode not in visited and\
               cost[neighNode] > nextCTG + edgeCost:
                # print("Node", neighNode, "From", nextVert)
                # print("New cost =", nextCTG + edgeCost)
                cost[neighNode] = nextCTG + edgeCost
                pred[neighNode] = nextVert
                q.update(cost[neighNode], neighNode)
    # This part is finding the best path from all nodes to the goal. Not necessary
    # for vert in graph.getVertices():
    #     bestPath = reconstructPath(goalVert, vert, pred)
    #     bestPath.reverse()
    #     print("Best path from ", vert, "to", goalVert, "is", bestPath)
    print("====> Dijkstra number of nodes visited: ", len(visited))
    print("====> Dijkstra number of nodes removed: ", num_q_nodes_removed)
    print("====> Dijkstra max size of queue: ", max_queue_size)
    finalPath = reconstructPath(goalVert, startVert, pred)
    finalPath.reverse()
    return finalPath
Ejemplo n.º 5
0
def AStarRoute(graph, startVert, goalVert):
    """ This algorithm searches a graph using Uniform Cost Search
    looking for a path from some start vertex to some goal vertex using
    lowest cost. It uses a PriorityQueue to store the indices of
    vertices that it still needs to examine."""
    maxQueueSize = 0
    nodesVisited = 0

    if startVert == goalVert:
        return []
    q = PriorityQueue()
    q.insert(graph.heuristicDist(startVert, goalVert), startVert)
    visited = {startVert}
    pred = {startVert: None}
    totalCost = {startVert: 0}
    while not q.isEmpty():
        if q.getSize() > maxQueueSize:
            maxQueueSize = q.getSize()

        nextCost, nextVert = q.firstElement()
        nodesVisited += 1

        nextCost = nextCost - graph.heuristicDist(nextVert, goalVert)
        if nextVert == goalVert:
            print("Total Cost is : " + str(nextCost))
            print("Total Nodes Visited : " + str(nodesVisited))
            print("Max queue size: " + str(maxQueueSize))
            return reconstructPath(startVert, goalVert, pred)
        q.delete()
        neighbors = graph.getNeighbors(nextVert)
        for (node, edgeCost) in neighbors:
            cost = nextCost + edgeCost
            # cost = nextCost + edgeCost + graph.heuristicDist(node, goalVert)

            if node not in visited:
                visited.add(node)
                pred[node] = nextVert
                totalCost[node] = cost
                q.insert(totalCost[node] + graph.heuristicDist(node, goalVert),
                         node)
            else:
                if cost < totalCost[node]:
                    pred[node] = nextVert
                    totalCost[node] = cost
                    q.insert(
                        totalCost[node] + graph.heuristicDist(node, goalVert),
                        node)

    return "NO PATH"
Ejemplo n.º 6
0
def AStarRoute(graph, startVert, goalVert):
    nodesRemoved = 0
    maxSize = 0
    if startVert == goalVert:
        return []

    minHeap = PriorityQueue()
    minHeap.insert(0, startVert)
    pred_cost = {}

    visited = set()
    pred = {}
    while not minHeap.isEmpty():
        if minHeap.size > maxSize:
            maxSize = minHeap.size
        nextVert = minHeap.firstElement()  # nextVert = [cost, vert]
        # print("--------------")
        # print("Popping", nextVert)
        minHeap.delete()
        nodesRemoved = nodesRemoved + 1
        if nextVert[1] in visited:
            continue
        else:
            visited.add(nextVert[1])
            if (nextVert[0], nextVert[1]) in pred_cost.keys():
                pred[nextVert[1]] = pred_cost[(nextVert[0], nextVert[1])]
            else:
                pred[nextVert[1]] = None
            if goalVert == nextVert[1]:
                return nodesRemoved, maxSize, reconstructPath(
                    startVert, goalVert, pred)
        neighbors = graph.getNeighbors(nextVert[1])
        # print('Adding neighbors to the queue: ')
        for n in neighbors:
            neighNode = n[0]
            edgeCost = n[1]
            if neighNode not in visited:
                Gcost = edgeCost + nextVert[0]
                Hcost = graph.heuristicDist(neighNode, goalVert)
                if startVert != nextVert[1]:
                    Gcost = Gcost - graph.heuristicDist(nextVert[1], goalVert)
                Fcost = Gcost + Hcost
                # print('Node ' + str(neighNode) + ' from ' + str(nextVert[1]))
                # print('G cost = ' + str(Gcost) + ' H cost = ' + str(Hcost) + ' F cost =  ' + str(Fcost))
                minHeap.insert(Fcost, neighNode)
                pred_cost[(Fcost, neighNode)] = nextVert[1]
    return "NO PATH"
Ejemplo n.º 7
0
def dijkstras(graph, startVert, goalVert):
    """ This algorithm searches a graph using Dijkstras algorithm to find
    the shortest path from every point to a goal point (actually
    searches from goal to every point, but it's the same thing.
    It uses a priority queue to store the indices of vertices that it still
    needs to examine.
    It returns the best path frmo startVert to goalVert, but otherwise
    startVert does not play into the search."""

    if startVert == goalVert:
        return []
    q = PriorityQueue()
    visited = set()
    pred = {}
    cost = {}
    for vert in graph.getVertices():
        cost[vert] = 1000.0
        pred[vert] = None
        q.insert(cost[vert], vert)
    visited.add(goalVert)
    cost[goalVert] = 0
    q.update(cost[goalVert], goalVert)
    while not q.isEmpty():
        (nextCTG, nextVert) = q.firstElement()
        q.delete()
        visited.add(nextVert)
        print("--------------")
        print("Popping", nextVert, nextCTG)
        neighbors = graph.getNeighbors(nextVert)
        for n in neighbors:
            neighNode = n[0]
            edgeCost = n[1]
            if neighNode not in visited and\
               cost[neighNode] > nextCTG + edgeCost:
                print("Node", neighNode, "From", nextVert)
                print("New cost =", nextCTG + edgeCost)
                cost[neighNode] = nextCTG + edgeCost
                pred[neighNode] = nextVert
                q.update( cost[neighNode], neighNode )
    for vert in graph.getVertices():
        bestPath = reconstructPath(goalVert, vert, pred)
        bestPath.reverse()
        print("Best path from ", vert, "to", goalVert, "is", bestPath)
    finalPath = reconstructPath(goalVert, startVert, pred)
    finalPath.reverse()
    return finalPath
Ejemplo n.º 8
0
class DStarAlgorithm:
    def __init__(self, graph, startVert, goalVert):
        """Takes in a graph, start vertex and goal vertex, and sets up the D* Lite
        search, initializing all the data structures and the priority queue."""
        self.graph = graph
        self.startVert = startVert
        self.goalVert = goalVert
        self.maxVal = math.inf
        self.initialize()

    def initialize(self):
        """The Initialize algorithm from the pseudocode."""
        self.U = PriorityQueue()
        self.nodesRemoved = 0
        self.maxSize = 0
        self.rhs = {}
        self.g = {}
        for node in self.graph.getVertices():
            self.rhs[node] = self.maxVal
            self.g[node] = self.maxVal
        self.rhs[self.startVert] = 0
        self.U.insert(
            self.calculateKey(self.startVert), self.startVert
        )  # The priority queue stores the priority first, then the vertex

    def computeShortestPath(self):
        """The ComputeShortestPath algorithm from the pseudocode."""

        while (not self.U.isEmpty()) and (self.compareKeys(
                self.U.firstElement()[0], self.calculateKey(
                    self.goalVert))) or (self.rhs[self.goalVert] !=
                                         self.g[self.goalVert]):
            if self.U.size > self.maxSize:
                self.maxSize = self.U.size
            u = self.U.firstElement()[1]
            self.U.delete()
            self.nodesRemoved = self.nodesRemoved + 1
            if self.g[u] > self.rhs[u]:
                self.g[u] = self.rhs[u]
            else:
                self.g[u] = self.maxVal
                self.updateVertex(u)
            successors = self.graph.getNeighbors(u)
            for s in successors:
                self.updateVertex(s[0])
        if self.U.isEmpty():
            return []  # So that it doesn't crash
        return self.reconstructPath()

    def updateVertex(self, vert):
        """The UpdateVertex algorithm from the pseudocode."""
        if vert != self.startVert:
            minVal = self.maxVal
            for s in self.graph.getNeighbors(vert):
                if self.g[s[0]] + s[1] < minVal:
                    minVal = self.g[s[0]] + s[1]
            self.rhs[vert] = minVal
        if self.U.contains(vert):
            self.U.removeValue(vert)
        if self.g[vert] != self.rhs[vert]:
            self.U.insert(self.calculateKey(vert), vert)

    def minNeighCost(self, vert):
        """A helper to compute the new rhs value, by finding the minimum cost among
        all the neighbors of a vertex. The cost is computed as the g cost of the
        neighbor plus the edge cost between the neighbor and the vertex."""
        minNCost = self.maxVal
        minVert = -1
        for neighInfo in self.graph.getNeighbors(vert):
            neigh = neighInfo[0]
            edgeCost = neighInfo[1]
            newCost = self.g[neigh] + edgeCost
            if newCost < minNCost:
                minNCost = newCost
                minVert = neigh
        return minNCost

    def calculateKey(self, vert):
        """Calculates the current priority for a given vertex"""
        minG = min(self.g[vert], self.rhs[vert])
        heurCost = self.graph.heuristicDist(vert, self.goalVert)
        return [minG + heurCost, minG]

    def compareKeys(self, key1, key2):
        """Takes in two keys, each of which is a list containing f cost
        and g cost. It prefers the lower f cost, but for equal f costs
        it chooses the lower g cost."""
        [f1, g1] = key1
        [f2, g2] = key2
        return (f1 < f2) or ((f1 == f2) and (g1 < g2))

    def correctInformation(self, newInfo):
        """Takes in a dictionary whose keys are (r, c) tuples, and the value
        is the newly corrected cell weight. Updates the graph, and then updates
        the search information appropriately."""
        for (r, c) in newInfo:
            self.graph.setCellValue(r, c, newInfo[r, c])
        self.graph.graphFromGrid()
        for (r, c) in newInfo:
            nodeNum = r * self.graph.getWidth() + c
            # print("(", r, c, ")", nodeNum)
            self.updateVertex(nodeNum)
            neighs = self.graph.getNeighbors(nodeNum)
            for (nextNeigh, wgt) in neighs:
                self.updateVertex(nextNeigh)

    def reconstructPath(self):
        """ Given the start vertex and goal vertex, and the table of
        predecessors found during the search, this will reconstruct the path
        from start to goal"""
        path = [self.goalVert]
        currVert = self.goalVert
        while currVert != self.startVert:
            currVert = self._pickMinNeighbor(currVert)
            path.insert(0, currVert)
        print(self.nodesRemoved)
        print(self.maxSize)
        return path

    def _pickMinNeighbor(self, vert):
        """A helper to path-reconstruction that finds the neighbor of a vertex
        that has the minimum g cost."""
        neighs = self.graph.getNeighbors(vert)
        minNeigh = None
        minVal = self.maxVal
        for [neigh, cost] in neighs:
            if self.g[neigh] < minVal:
                minVal = self.g[neigh]
                minNeigh = neigh
        return minNeigh
Ejemplo n.º 9
0
class DStarAlgorithm:
    def __init__(self, graph, startVert, goalVert):
        """Takes in a graph, start vertex and goal vertex, and sets up the D* Lite
        search, initializing all the data structures and the priority queue."""
        self.graph = graph
        self.startVert = startVert
        self.goalVert = goalVert
        self.maxVal = 1000

        # PriorityQueue with values [fEst, gEst]
        self.U = PriorityQueue()
        # estimated cost from g and c for a vertex
        self.rhs = {}
        # estimated cost from start vertex to other vertex
        self.g = {}

        # print(self.startVert, self.goalVert)

        self.initialize()

    def initialize(self):
        """The Initialize algorithm from the pseudocode."""
        for s in range(0, self.graph._numVerts):
            # s = self.graph.getData(i)
            self.rhs.update({s: self.maxVal})
            self.g.update({s: self.maxVal})

        self.rhs[self.startVert] = 0
        # print(self.g, self.rhs)
        # print(self.g.values(),self.rhs.values())

        self.U.insert(self.calculateKey(self.startVert), self.startVert)

    def computeShortestPath(self):
        """The ComputeShortestPath algorithm from the pseudocode."""
        # print(self.rhs[self.goalVert])
        # print(self.g[self.goalVert])
        # print((self.rhs[self.goalVert] != self.g[self.goalVert]))

        while (self.U.firstElement() is not None) and (
            (self.U.firstElement()[0][0] < self.calculateKey(self.goalVert)[0])
                or (self.rhs[self.goalVert] != self.g[self.goalVert])):
            u = self.U.firstElement()[1]
            # print(u)
            self.U.delete()
            if self.g.get(u) > self.rhs.get(u):
                self.g[u] = self.rhs[u]
            else:
                self.g[u] = np.inf
                self.updateVertex(u)
            for [s, cost] in self.graph.getNeighbors(u):
                self.updateVertex(s)

        return self.reconstructPath()

        # if (self.U.firstElement() is not None)
        #     break

    def updateVertex(self, vert):
        """The UpdateVertex algorithm from the pseudocode."""
        if vert != self.startVert:
            self.rhs[vert] = self.minNeighCost(vert)
        if self.U.contains(vert):
            self.U.removeValue(vert)
        if self.g[vert] != self.rhs[vert]:
            self.U.insert(self.calculateKey(vert), vert)

    def minNeighCost(self, vert):
        """A helper to compute the new rhs value, by finding the minimum cost among
        all the neighbors of a vertex. The cost is computed as the g cost of the
        neighbor plus the edge cost between the neighbor and the vertex."""
        minNCost = self.maxVal
        minVert = -1
        for neighInfo in self.graph.getNeighbors(vert):
            neigh = neighInfo[0]
            edgeCost = neighInfo[1]
            newCost = self.g[neigh] + edgeCost
            if newCost < minNCost:
                minNCost = newCost
                minVert = neigh
        return minNCost

    def calculateKey(self, vert):
        """The CalculateKey algorithm from the pseudocode."""
        minG = min(self.g.get(vert), self.rhs.get(vert))
        heurCost = self.graph.heuristicDist(vert, self.goalVert)
        return [minG + heurCost, minG]

    def compareKeys(self, key1, key2):
        """Takes in two keys, each of which is a list containing f cost
        and g cost. It prefers the lower f cost, but for equal f costs
        it chooses the lower g cost."""
        [f1, g1] = key1
        [f2, g2] = key2
        return (f1 < f2) or ((f1 == f2) and (g1 < g2))

    def correctInformation(self, newInfo):
        """Takes in a dictionary whose keys are (r, c) tuples, and the value
        is the newly corrected cell weight. Updates the graph, and then updates
        the search information appropriately."""
        for (r, c) in newInfo:
            self.graph.setCellValue(r, c, newInfo[r, c])
        self.graph.graphFromGrid()
        for (r, c) in newInfo:
            nodeNum = r * self.graph.getWidth() + c
            #print("(", r, c, ")", nodeNum)
            self.updateVertex(nodeNum)
            neighs = self.graph.getNeighbors(nodeNum)
            for (nextNeigh, wgt) in neighs:
                self.updateVertex(nextNeigh)

    def reconstructPath(self):
        """ Given the start vertex and goal vertex, and the table of
        predecessors found during the search, this will reconstruct the path
        from start to goal"""

        path = [self.goalVert]
        currVert = self.goalVert
        while currVert != self.startVert:
            currVert = self._pickMinNeighbor(currVert)
            path.insert(0, currVert)
        return path

    def _pickMinNeighbor(self, vert):
        """A helper to path-reconstruction that finds the neighbor of a vertex
        that has the minimum g cost."""
        neighs = self.graph.getNeighbors(vert)
        minVal = self.maxVal
        minNeigh = None
        for [neigh, cost] in neighs:
            if self.g[neigh] < minVal:
                minVal = self.g[neigh]
                minNeigh = neigh
        return minNeigh