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
parentTheta=[-1 for x in range(graph.getNumVertices())]
parentV=[-1 for x in range(graph.getNumVertices())]
#run A*
a_star=A_star.A_star(graph,[])
parent=a_star.run(start, goal)
#run Theta*
theta_star=Theta_star.Theta_star(graph,obstacles)
parentTheta=theta_star.run(start, goal)
#check if anyone found path
child=functions.getVertexID(goal[0], goal[1])
parents=parent[child]
parentT=parentTheta[child]
if (parents==-1 and parentT==-1):
print "No paths found!"
sys.exit()
#############################################################################
#############################################################################
#############################################################################
#run A* on visibility graph
corners=[start,goal]
for i in range (len(obstacles)):
value=obstacles[i]
def lineOfSight(self, s, sPrime, obstacles):
#print "\nstarting line of sight"
lineOk=True
temp=self.graph.getVertex(s)
s_x=functions.convertColToPixel(temp[0])
s_y=functions.convertRowToPixel(temp[1])
temp=self.graph.getVertex(sPrime)
sPrime_x=functions.convertColToPixel(temp[0])
sPrime_y=functions.convertRowToPixel(temp[1])
#print "s's x,y location: " +str(s_x) +", " + str(s_y)
#print "sprim's x,y location: " +str(sPrime_x) +", " + str(sPrime_y)
if(s<sPrime):
loc_x=float(s_x)
loc_y=float(s_y)
end_x=float(sPrime_x)
end_y=float(sPrime_y)
else:
loc_x=float(sPrime_x)
loc_y=float(sPrime_y)
end_x=float(s_x)
end_y=float(s_y)
if(end_x>loc_x):
whichWay=True
else:
whichWay=False
if((sPrime_y-s_y)!=0 and (sPrime_x-s_x)!=0):
#print "slope!"
slope=float((sPrime_y-s_y))/(float(sPrime_x-s_x))
b=s_y-(slope*s_x)
#print "b: " +str(b) + " slope: " +str(slope)
while(not(round(loc_x,10)==round(end_x,10) and round(loc_y, 10)==round(end_y, 10))):
#print "Pix_x: "+str(loc_x) + " Pix_y: " + str(loc_y)
#print "Pix_x: "+str(end_x) + " Pix_y: " + str(end_y)
#print "Pix_x: "+str(round(loc_x,10)==round(end_x,10)) + " Pix_y: " + str(round(loc_y, 10)==round(end_y, 10))
if(((loc_x%1.0)==0.0 and (loc_x%1.0)==0.0) and int(loc_x)==int(end_x) and int(loc_y)==int(end_y)):
break
if(float(loc_x)<0 or float(loc_y)<0):
print "ERROR:Pixels in negative"
sys.exit()
x=functions.convertPixelToCol(loc_x)
y=functions.convertPixelToRow(loc_y)
checkX=x
checkY=y
#check for blocked
if (not((checkX%1.0)==0.0 or (checkY%1.0)==0.0) and (checkX+1)<(colLength-1) and (checkY+1)<(rowLength-1)):
#print "x: "+str(x) + " y: " + str(y) + " not whole"
#print str(checkX%1.0)+ " " + str(checkY%1.0)
#print "x: "+str(math.floor(checkX)) + " y: " + str(math.floor(checkY)) + " not whole"
if not(self.graph.getEdge(functions.getVertexID(int(math.floor(checkX)), int(math.floor(checkY))), functions.getVertexID(int(math.floor(checkX+1)), int(math.floor(checkY+1))))>0):
#print "doesn't work"
return False
elif (not((checkX%1.0)==0.0 and (checkY%1.0)==0.0)):
for i in range (len(obstacles)):
value=[int(math.floor(checkX)),int(math.floor(checkY))]
if obstacles[i]==[int(math.floor(checkX)),int(math.floor(checkY))]:
return False
#update values
if(loc_x!=end_x):
if(whichWay):
loc_x=loc_x+.25
else:
loc_x=loc_x-.25
loc_y=(loc_x*slope)+b
elif(loc_y!=end_y):
loc_y=loc_y+.25
loc_x=(loc_y-b)/slope
elif((sPrime_x-s_x)==0):
#print "vertical line"
while(not(round(loc_x,10)==round(end_x,10) and round(loc_y, 10)==round(end_y, 10))):
#print "Pix_x: "+str(loc_x) + " Pix_y: " + str(loc_y)
x=functions.convertPixelToCol(loc_x)
y=functions.convertPixelToRow(loc_y)
checkX=x
checkY=y
if(((loc_x%1.0)==0.0 and (loc_x%1.0)==0.0) and int(loc_x)==int(end_x) and int(loc_y)==int(end_y)):
break
if (not((checkX%1.0)==0.0 or (checkY%1.0)==0.0) and (checkX+1)<(colLength-1) and (checkY+1)<(rowLength-1)):
#print "x: "+str(x) + " y: " + str(y) + " not whole"
#print str(x%1.0)+ " " + str(y%1.0)
#print "x: "+str(math.floor(checkX)) + " y: " + str(math.floor(checkY)) + " not whole"
if not(self.graph.getEdge(functions.getVertexID(int(math.floor(checkX)), int(math.floor(checkY))), functions.getVertexID(int(math.floor(checkX)+1), int(math.floor(checkY)+1)))>0):
#print "doesn't work"
return False
elif (not((checkX%1.0)==0.0 and (checkY%1.0)==0.0)):
for i in range (len(obstacles)):
value=[int(math.floor(checkX)),int(math.floor(checkY))]
if obstacles[i]==[int(math.floor(checkX)),int(math.floor(checkY))]:
return False
#update next value
loc_y=loc_y+.25
elif((sPrime_y-sPrime_y)==0):
#print "horizontal line"
while(not(round(loc_x,10)==round(end_x,10) and round(loc_y, 10)==round (end_y, 10))):
#print "Pix_x: "+str(loc_x) + " Pix_y: " + str(loc_y)
x=functions.convertPixelToCol(loc_x)
y=functions.convertPixelToRow(loc_y)
checkX=x
checkY=y
if(((loc_x%1.0)==0.0 and (loc_x%1.0)==0.0) and int(loc_x)==int(end_x) and int(loc_y)==int(end_y)):
break
#check for blocked
if (not((checkX%1.0)==0.0 or (checkY%1.0)==0.0) and (checkX+1)<(colLength-1) and (checkY+1)<(rowLength-1)):
#print "x: "+str(x) + " y: " + str(y) + " not whole"
#print str(x%1.0)+ " " + str(y%1.0)
#print "x: "+str(math.floor(checkX)) + " y: " + str(math.floor(checkY)) + " not whole"
if not(self.graph.getEdge(functions.getVertexID(int(math.floor (checkX)), int(math.floor(checkY))), functions.getVertexID (int(math.floor(checkX)+1), int(math.floor(checkY)+1)))>0):
#print "doesn't work"
return False
elif (not((checkX%1.0)==0.0 and (checkY%1.0)==0.0)):
for i in range (len(obstacles)):
value=[int(math.floor(checkX)),int(math.floor(checkY))]
if obstacles[i]==[int(math.floor(checkX)),int(math.floor(checkY))]:
return False
#update next value
loc_x=loc_x+.25
#print "end of line of sight\n"
return lineOk
