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