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