87 lines
2.3 KiB
Python
87 lines
2.3 KiB
Python
import numpy as np
|
|
|
|
class node:
|
|
|
|
def __init__(self, x, y):
|
|
self.x = x
|
|
self.y = y
|
|
self.neighbours = []
|
|
self.parent = None
|
|
self.g = 0
|
|
self.f = 0
|
|
self.h = 0
|
|
|
|
def makeNeighbours(self, matrix):
|
|
|
|
if self.x - 1 >= 0 and matrix[self.x - 1, self.y] != 3:
|
|
self.neighbours.append([self.x - 1, self.y])
|
|
|
|
if self.x + 1 < matrix.shape[0] and matrix[self.x + 1, self.y] != 3:
|
|
self.neighbours.append([self.x + 1, self.y])
|
|
|
|
if self.y + 1 < matrix.shape[1] and matrix[self.x, self.y + 1] != 3:
|
|
self.neighbours.append([self.x, self.y + 1])
|
|
|
|
if self.y - 1 >= 0 and matrix[self.x, self.y - 1] != 3:
|
|
self.neighbours.append([self.x, self.y - 1])
|
|
|
|
def calcHeuristic(self, goal):
|
|
self.h = abs(self.x - goal.x) + abs(self.y - goal.y)
|
|
|
|
def Astar(A, start, goal):
|
|
frontier = []
|
|
|
|
objects = {}
|
|
|
|
for i in range(A.shape[0]):
|
|
for j in range(A.shape[1]):
|
|
objects[i, j] = node(i, j)
|
|
|
|
objects[0, 0].visited = True
|
|
goal = objects[goal[0], goal[1]]
|
|
start = objects[start[0], start[1]]
|
|
|
|
frontier.append(start)
|
|
|
|
open = []
|
|
closed = []
|
|
|
|
open.append(start)
|
|
|
|
while open:
|
|
open.sort(key=lambda o: o.f)
|
|
|
|
current = open.pop(0)
|
|
|
|
closed.append(current)
|
|
|
|
current.makeNeighbours(A)
|
|
|
|
if current == goal:
|
|
break
|
|
|
|
for next in current.neighbours:
|
|
if objects[next[0], next[1]] in closed:
|
|
continue
|
|
|
|
if objects[next[0], next[1]] in open:
|
|
new_g = current.g + 1
|
|
if objects[next[0], next[1]].g > new_g:
|
|
objects[next[0], next[1]].g = new_g
|
|
objects[next[0], next[1]].parent = objects[current.x, current.y]
|
|
|
|
else:
|
|
objects[next[0], next[1]].g = objects[current.x, current.y].g + 1
|
|
objects[next[0], next[1]].calcHeuristic(goal)
|
|
objects[next[0], next[1]].parent = objects[current.x, current.y]
|
|
open.append(objects[next[0], next[1]])
|
|
|
|
path = []
|
|
while current.parent:
|
|
path.append([current.x, current.y])
|
|
current = current.parent
|
|
|
|
path.append([start.x, start.y])
|
|
path.reverse()
|
|
|
|
return path |