python - A* pathfinding on 2D grid doesn't find optimal path -
i trying implement a* algorithm on 2d square grid. however, never finds optimal path, , can't see why. code suspiciously slow, python.
i've tried anything, i'm out of ideas.
this have far:
file astar.py:
end = none nlut = [ (1,0) , (0,1) , (-1,0) , (0,-1) ] class tile: def __init__(self,x,y,g=0,parent=none): self.x = x self.y = y self.g = g self.parent = parent def __eq__(self,other): if other == none: return false return ( (self.x == other.x) , (self.y == other.y)) def __ne__(self,other): return not self.__eq__(other) def __hash__(self): return hash((self.x,self.y)) def __str__(self): if self.parent == none: sss = "" else: sss = " <- "+str(self.parent.coords()) return "<"+str(self.x)+"."+str(self.y)+"g"+str(self.g)+">"+sss def __repr__(self): return "<"+str(self.x)+"."+str(self.y)+">" def coords(self): return (self.x,self.y) def f(self): return self.g + self.h() def h(self): global end return ((abs(self.x-end[0]) + abs(self.y-end[1]))) def nbd(self): global nlut return [ tile(self.x + n[0], self.y + n[1], self.g + 1, self) n in nlut] def pathfind(mapfunction,start,endp): global end end = endp debug = false # debug = true def log(st): if debug: print(st) startile = tile(start[0],start[1],0) endtile = tile(end[0],end[1]) path = [] # init openset starting tile openset = set([startile]) closedset = set() stepcount = 0 #the loop, long openset not empty: while len(openset)>0: log(str(stepcount)+"-"+str(openset)) stepcount += 1 fcur = float("inf") #find lowest f-count tile in open set o in openset: if o.f() < fcur: fcur = o.f() current = o log("current: "+str(current)) #move current tile closed set openset.remove(o) closedset.add(o) #find von neumann neighbourhood nbd = o.nbd() log("nbd: "+str(nbd)) #work on neighbours n in nbd: log("processing "+str(n)) if mapfunction(n.x,n.y) != 0: #if it's blocked, ignore log("it blocked.") continue elif n in closedset: #if it's in closed set, ignore log("it in closed set.") continue elif n in openset: #if it's in open set... log("it's in open set...") e in openset: if n==e: #find old copy of n in open set if n.g < e.g: #if it's better path, substitute log("substitution") openset.remove(e) openset.add(n) else: log("old path better.") break #no need go on... else: #if it's not in open set, add log("not in open set, adding...") openset.add(n) if endtile in closedset: #if we're done #find copy of endtile in closedset e in closedset: if e == endtile: rec = e while (not rec == none) , rec != startile: #reconstruct path path.append(rec.coords()) rec = rec.parent path.append(startile.coords()) return path #if openset empty, no path found. :( return -1 and little demo program, astardemo.py:
import astar random import * mapp = [[randint(0,10)//10 _ in range(0,50)] _ in range(0,50)] def isblocked(x,y): global mapp if not (x in range(0,50) , y in range(0,50)): return 1 return (mapp[x][y] != 0) start = (randint(0,49),randint(0,49)) end = (randint(0,49),randint(0,49)) mapp[start[0]][start[1]] = 0 mapp[end[0]][end[1]] = 0 p = astar.pathfind(isblocked,start,end) print p l = "" in range(0,50): j in range(0,50): if (i,j) in p: if mapp[i][j] > 0: l+="e" else: l+=str(p.index((i,j))%10) elif mapp[i][j] > 0: l+="#" else: l+=" " l+="\n" print l
o in openset: if o.f() < fcur: fcur = o.f() current = o log("current: "+str(current)) #move current tile closed set openset.remove(o) closedset.add(o) #find von neumann neighbourhood nbd = o.nbd() in code use o outside loop, last value in loop did mean use current?
o in openset: if o.f() < fcur: fcur = o.f() current = o log("current: "+str(current)) #move current tile closed set openset.remove(current) closedset.add(current) #find von neumann neighbourhood nbd = current.nbd() 
Comments
Post a Comment