Ejemplo n.º 1
0
 def UpdateVertexTheta(self,s, sPrime, queue, goal):
     val=self.graph.getVertex(sPrime)
     parentS=int(self.parentTheta[s])
     parentSVal=self.graph.getVertex(parentS)
 
     #print "UpdateVertex: " + str(s) + " " + str(sPrime) + " " + str(self.parentTheta[s])
     if( self.lineOfSight( self.parentTheta[s], sPrime,self.obstacles) ==True):
         # Path 2
         if((self.pathCostT[parentS]+functions.getDistance(parentSVal[0], parentSVal[1], val[0], val[1])) < self.pathCostT[sPrime]):
             self.pathCostT[sPrime] = self.pathCostT[parentS]+functions.getDistance(parentSVal[0], parentSVal[1], val[0], val[1])
             self.parentTheta[sPrime] = self.parentTheta[s]
             #print str(self.parentTheta[sPrime]) + " " + str(sPrime)
         
             #  insert into open list
             checkVertex=self.graph.getVertex(sPrime)
             if(checkVertex[3]==1):
                 cpyQueue=queue
                 queue=functions.removeFromHeap(cpyQueue, sPrime)
             distance=functions.getDistanceToGoal(val[0],val[1], goal)
             queue.push([self.pathCostT[sPrime]+distance,self.pathCostT[sPrime], sPrime, True])
         
             #mark it, and give it f, g, h values
             VertexInfo=self.graph.getVertex(sPrime)
             VertexInfo[3]=1
             VertexInfo[7]=self.pathCostT[sPrime]
             VertexInfo[8]=distance
             VertexInfo[9]=self.pathCostT[sPrime]+distance
             self.graph.setVertex(sPrime, VertexInfo)
 
     else:
         # Path 1
         sVal=self.graph.getVertex(s)
         if( ( self.pathCostT[s]+functions.getDistance(sVal[0], sVal[1], val[0], val[1])) < self.pathCostT[sPrime] ):
             self.pathCostT[sPrime] = self.pathCostT[s]+functions.getDistance(sVal[0], sVal[1], val[0], val[1])
             self.parentTheta[sPrime] =s
             #print str(self.parentTheta[sPrime]) + " " + str(sPrime)
         
             #  insert into open list
             checkVertex=self.graph.getVertex(sPrime)
             if(checkVertex[3]==1):
                 cpyQueue=queue
                 queue=functions.removeFromHeap(cpyQueue, sPrime)
             distance=functions.getDistanceToGoal(val[0],val[1], goal)
             queue.push([self.pathCostT[sPrime]+distance,self.pathCostT[sPrime], sPrime, True])
         
             #mark vertex, and assign f,g, h
             VertexInfo=self.graph.getVertex(sPrime)
             VertexInfo[3]=1
             VertexInfo[7]=self.pathCostT[sPrime] #g
             VertexInfo[8]=distance #h
             VertexInfo[9]=self.pathCostT[sPrime]+distance
             self.graph.setVertex(sPrime, VertexInfo)
     return queue
Ejemplo n.º 2
0
    def run(self, start, goal):
        self.pathCost[functions.getVertexID(start[0], start[1])]=0 #start following algorithm
        self.parent[functions.getVertexID(start[0], start[1])]=functions.getVertexID(start[0], start[1])

        #push start into the queue --> heap assign an array as a value to each item. The array has the f(item), and the item's vertex id

        self.fringe.push([self.pathCost[functions.getVertexID(start[0], start[1])]+functions.getDistanceToGoal(start[0], start[1], goal),self.pathCost[functions.getVertexID(start[0], start[1])], functions.getVertexID(start[0], start[1]), True])
        
        #mark it as "added to self.fringe" by changing thrid # to 1
        VertexInfo=self.graph.getVertex(functions.getVertexID(start[0], start[1]))
        VertexInfo[3]=1
        self.graph.setVertex(functions.getVertexID(start[0], start[1]), VertexInfo)
            
        #start the A* loop
        print "\nStarting A*"
        startTimeA=time.time()
        while (self.fringe.len)>0:
            #pop first value of queue --> checkout the beginning of "making a self.graph" to find out what's in the array
            vrtx=self.fringe.pop()
            #print "vrtx: " + str(vrtx)
            if(vrtx==-1):
                self.pathFound=False
                break
    
            #if it's goal, then break
            if(vrtx[2]==functions.getVertexID(goal[0], goal[1])):
                print "A* found path!"
                print "Time taken: " +str(time.time()-startTimeA)
                vrtInfo=self.graph.getVertex(vrtx[2])
                print "Total cost: " +str(vrtInfo[4])
                self.pathFound=True
                #self.write_results(time.time()-startTimeA, vrtInfo[4], False)
                break
    
            #mark vertex as closed
            VertexInfo=self.graph.getVertex(vrtx[2])
            VertexInfo[2]=0
            self.graph.setVertex(vrtx[2], VertexInfo)
    
            #get the neighbors
            ngbrs=[]
            ngbrs=self.graph.neighbourOf(vrtx[2])
            #print "neighbors: " + str(ngbrs)
            #loop through neighbors
            for i in range (len(ngbrs)):
                checkNeighbor=self.graph.getVertex(ngbrs[i])
                if(checkNeighbor[2]==1):
                    if(checkNeighbor[3]==0):
                        self.pathCost[ngbrs[i]]=1000000000
                        self.parent[ngbrs[i]]=-1
                    cpyfringe=self.fringe
                    self.fringe=self.UpdateVertex(vrtx[2],ngbrs[i], cpyfringe, goal)

        if(self.pathFound!=True):
            print "A* did not find path!"
            return [-1 for x in range(self.graph.getNumVertices())]
        else:
            return self.parent
Ejemplo n.º 3
0
 def UpdateVertex(self,s, sPrime, queue,goal):
     val=self.graph.getVertex(sPrime)
     if((self.pathCost[s]+self.graph.getEdge(s, sPrime))<self.pathCost[sPrime]):
         self.pathCost[sPrime]=self.pathCost[s]+self.graph.getEdge(s, sPrime)
         self.parent[sPrime]=s
         
         #remove from queue
         cpyQueue=queue
         queue=functions.removeFromHeap(cpyQueue, sPrime)
         distance=functions.getDistanceToGoal(val[0],val[1], goal)
         queue.push([self.pathCost[sPrime]+distance,self.pathCost[sPrime], sPrime, True])
         VertexInfo=self.graph.getVertex(sPrime)
         VertexInfo[3]=1
         VertexInfo[4]=self.pathCost[sPrime]
         VertexInfo[5]=distance
         VertexInfo[6]=self.pathCost[sPrime]+distance
         self.graph.setVertex(sPrime, VertexInfo)
     
     #print "cost of: " + str(sPrime) + " " + str(self.pathCost[sPrime]+getDistanceToGoal(val[0],val[1]))
     return queue