109 lines
2.3 KiB
Python
109 lines
2.3 KiB
Python
from numpy import Infinity
|
|
|
|
|
|
CELL = 0
|
|
WALL = 1
|
|
START = 2
|
|
END = 3
|
|
|
|
|
|
ROWS = 10
|
|
COLS = 10
|
|
|
|
|
|
class Node:
|
|
def __init__(self, x, y) -> None:
|
|
self.x = x
|
|
self.y = y
|
|
self.gscore = Infinity
|
|
self.fscore = Infinity
|
|
self.celltype = CELL
|
|
|
|
def __str__(self) -> str:
|
|
return f"({self.x}, {self.y})"
|
|
|
|
def __repr__(self) -> str:
|
|
return f"({self.x}, {self.y})"
|
|
|
|
def g_pos(self):
|
|
return (self.x, self.y)
|
|
|
|
def s_gscore(self, g):
|
|
self.gscore = g
|
|
|
|
def s_fscore(self, f):
|
|
self.fscore = f
|
|
|
|
def g_gscore(self):
|
|
return self.gscore
|
|
|
|
def g_fscore(self):
|
|
return self.fscore
|
|
|
|
def g_neighbours(self, grid):
|
|
neighours = []
|
|
if self.x > 0:
|
|
neighours.append(grid[self.x - 1][self.y])
|
|
|
|
if self.x < ROWS - 1:
|
|
neighours.append(grid[self.x + 1][self.y])
|
|
|
|
if self.y > 0:
|
|
neighours.append(grid[self.x][self.y - 1])
|
|
|
|
if self.y < COLS - 1:
|
|
neighours.append(grid[self.x][self.y + 1])
|
|
|
|
return filter(lambda n: n.celltype != WALL, neighours)
|
|
|
|
|
|
def get_best(openset):
|
|
if len(openset) == 1:
|
|
return openset[0]
|
|
|
|
sorted = openset
|
|
sorted.sort(key=lambda c: c.fscore)
|
|
|
|
return sorted[0]
|
|
|
|
|
|
def manhatan_distance(node, end):
|
|
return abs(node.x - end.x) + abs(node.y - end.y)
|
|
|
|
|
|
def heuristic(node, end):
|
|
return manhatan_distance(node, end)
|
|
|
|
|
|
def reconstruct_path(cameFrom, current):
|
|
path = [current]
|
|
while current in cameFrom:
|
|
current = cameFrom[current]
|
|
path.append(current)
|
|
return path
|
|
|
|
|
|
def a_star(grid, start, end):
|
|
openset = [start]
|
|
cameFrom = dict()
|
|
|
|
start.s_gscore(0)
|
|
start.s_fscore(heuristic(start, end))
|
|
|
|
while len(openset) != 0:
|
|
current = get_best(openset)
|
|
if current.celltype == END:
|
|
return reconstruct_path(cameFrom, current)
|
|
|
|
openset.remove(current)
|
|
for n in current.g_neighbours(grid):
|
|
temp_gscore = current.g_gscore() + 1
|
|
if temp_gscore < n.g_gscore():
|
|
cameFrom[n] = current
|
|
n.s_gscore(temp_gscore)
|
|
n.s_fscore(temp_gscore + heuristic(n, end))
|
|
if n not in openset:
|
|
openset.append(n)
|
|
|
|
return None
|