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