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
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
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