Как уже указывал @ leni c, внутренние циклы, которые у вас есть, не принадлежат алгоритму A *.
Первый (for node in closedList) должен быть заменен проверкой того, узел находится в наборе (а не в списке): таким образом вам не потребуется итерация.
Второй узел (openNode in openList) не нужен, когда вы инициализируете все значения свойств g бесконечностью (кроме начальный узел). Затем вы можете просто сравнить новое значение g с тем, которое уже хранится в узле neighbor.
Кроме того, я бы предложил создавать узлы для всего графа сразу после его создания. Это будет полезно, когда вам нужно выполнить несколько запросов на одном и том же графике.
Кроме того, вместо bisect.insort_left я бы предложил использовать heapq.heappush. Это будет быстрее, так как на самом деле он не сортирует очередь полностью, а просто обеспечивает сохранение свойства кучи. Однако временная сложность такая же. Что еще более важно, временная сложность получения следующего значения из него лучше, чем openList.pop(0).
Я бы посоветовал работать со стоимостью 10, 13 и 15 вместо 1, 1,3 и 1,5. , поскольку у целочисленных операций нет проблем с точностью.
По той же причине я не стал бы работать с дробными координатами местоположения. Поскольку все они так же далеко друг от друга (например, 0,002), мы можем просто использовать последовательные целые числа (0, 1, 2, ...) для обеих координат. Дополнительная функция может принимать решение и опорную пару координат для преобразования этих целочисленных координат обратно в «мировые» координаты.
Я сделал несколько предположений:
Это строго запрещено go через "враждебные" ячейки. Для их пересечения не будет никаких ребер. Например, если начальная локация находится в середине четырех враждебных ячеек, пути не будет. Можно было бы рассмотреть альтернативу, в которой эти края будут иметь чрезвычайно высокую стоимость, чтобы вы всегда могли предложить путь.
Прямые края на границе сетки - это все разрешается. Их стоимость будет 1,3 (13), когда соседняя с ней одна ячейка является враждебной. Таким образом, на самом деле в каждом из двух измерений есть на одно местоположение больше, чем количество ячеек
Пороговый вход будет дробью от 0 до 1, что указывает на долю ячеек, которая должна быть дружественный по отношению к общему количеству ячеек, и это значение будет преобразовано в "разделенное" значение, чтобы различать guish между дружественными и враждебными ячейками.
Вот код, который вы могли бы использовать в качестве вдохновения:
from heapq import heappop, heappush
class Node:
def __init__(self, location):
self.location = location
self.neighbors = []
self.parent = None
self.g = float('inf')
self.f = 0
def clear(self):
self.parent = None
self.g = float('inf')
self.f = 0
def addneighbor(self, cost, other):
# add edge in both directions
self.neighbors.append((cost, other))
other.neighbors.append((cost, self))
def __gt__(self, other): # make nodes comparable
return self.f > other.f
def __repr__(self):
return str(self.location)
class Graph:
def __init__(self, grid, thresholdfactor):
# get value that corresponds with thresholdfactor (which should be between 0 and 1)
values = sorted([value for row in grid for value in row])
splitvalue = values[int(len(values) * thresholdfactor)]
print("split at ", splitvalue)
# simplify grid values to booleans and add extra row/col of dummy cells all around
width = len(grid[0]) + 1
height = len(grid) + 1
colors = ([[False] * (width + 1)] +
[[False] + [value < splitvalue for value in row] + [False] for row in grid] +
[[False] * (width + 1)])
nodes = []
for i in range(height):
noderow = []
nodes.append(noderow)
for j in range(width):
node = Node((i, j))
noderow.append(node)
cells = [colors[i+1][j]] + colors[i][j:j+2] # 3 cells around location: SW, NW, NE
for di, dj in ((1, 0), (0, 0), (0, 1), (0, 2)): # 4 directions: W, NW, N, NE
cost = 0
if (di + dj) % 2: # straight
# if both cells are hostile, then not allowed
if cells[0] or cells[1]: # at least one friendly cell
# if one is hostile, higher cost
cost = 13 if cells[0] != cells[1] else 10
cells.pop(0)
elif cells[0]: # diagonal: cell must be friendly
cost = 15
if cost:
node.addneighbor(cost, nodes[i-1+di][j-1+dj])
self.nodes = nodes
@staticmethod
def reconstructpath(node):
path = []
while node is not None:
path.append(node)
node = node.parent
path.reverse()
return path
@staticmethod
def heuristic(a, b):
# optimistic score, assuming all cells are friendly
dy = abs(a[0] - b[0])
dx = abs(a[1] - b[1])
return min(dx, dy) * 15 + abs(dx - dy) * 10
def clear(self):
# remove search data from graph
for row in self.nodes:
for node in row:
node.clear()
def a_star(self, start, end):
self.clear()
startnode = self.nodes[start[0]][start[1]]
endnode = self.nodes[end[0]][end[1]]
startnode.g = 0
openlist = [startnode]
closed = set()
while openlist:
node = heappop(openlist)
if node in closed:
continue
closed.add(node)
if node == endnode:
return self.reconstructpath(endnode)
for weight, neighbor in node.neighbors:
g = node.g + weight
if g < neighbor.g:
neighbor.g = g
neighbor.f = g + self.heuristic(neighbor.location, endnode.location)
neighbor.parent = node
heappush(openlist, neighbor)
Я закодировал график, который вы включили в качестве изображения, чтобы увидеть, как будет вести себя код:
grid = [
[38, 32, 34, 24, 0, 82, 5, 41, 11, 32, 0, 16, 0,113, 49, 34, 24, 6, 15, 35],
[61, 61, 8, 35, 65, 31, 53, 25, 66, 0, 21, 0, 9, 0, 31, 75, 20, 8, 3, 29],
[43, 66, 47,114, 38, 41, 1,108, 9, 0, 0, 0, 39, 0, 27, 72, 19, 14, 24, 25],
[45, 5, 37, 23,102, 25, 49, 34, 41, 49, 35, 15, 29, 21, 66, 67, 44, 31, 38, 91],
[47, 94, 48, 69, 33, 95, 18, 75, 28, 70, 38, 78, 48, 88, 21, 66, 44, 70, 75, 23],
[23, 84, 53, 23, 92, 14, 71, 12,139, 30, 63, 82, 16, 49, 76, 56,119,100, 47, 21],
[30, 0, 32, 90, 0,195, 85, 65, 18, 57, 47, 61, 40, 32,109,255, 88, 98, 39, 0],
[ 0, 0, 0, 0, 39, 39, 76,167, 73,140, 58, 56, 94, 61,212,222,141, 50, 41, 20],
[ 0, 0, 0, 5, 0, 0, 21, 2,132,100,218, 81, 0, 62,135, 42,131, 80, 14, 19],
[ 0, 0, 0, 0, 0, 15, 9, 55, 70, 71, 42,117, 65, 63, 59, 81, 4, 40, 77, 46],
[ 0, 0, 0, 0, 55, 52,101, 93, 30,166, 56, 19, 76,103, 54, 37, 24, 23, 59, 98],
[ 0, 0, 0, 0, 9, 9, 44,149, 11,134, 90, 64, 44, 57, 61, 79,270,201, 84, 6],
[ 0, 0, 0, 22, 1, 15, 0, 25, 30,101,154, 60, 97, 64, 15,162, 27, 91, 71, 0],
[ 0, 0, 1, 35, 5, 10, 0, 55, 25, 0,200, 81, 31, 53, 42, 74,127,154, 7, 0],
[ 0, 0,187, 17, 45, 66, 91,191, 70,189, 18, 25, 67, 32, 40, 79,103, 79, 59, 0],
[ 0, 21, 16, 14, 19, 58,278, 56,128, 95, 3, 52, 9, 27, 25, 43, 62, 25, 38, 0],
[ 4, 3, 11, 26,119,165, 53, 85, 46, 81, 19, 11, 12, 19, 18, 9, 16, 6, 37, 0],
[ 5, 0, 0, 65,158,153,118, 38,123, 46, 28, 24, 0, 21, 11, 20, 5, 1, 10, 0],
[17, 4, 28, 81,101,101, 46, 25, 44, 12, 41, 6, 27, 8, 4, 32, 40, 1, 1, 0],
[26, 20, 84, 42,112, 27, 14, 16, 5, 13, 3, 43, 6, 18, 12, 44, 5, 0, 0, 5]
]
graph = Graph(grid, 0.5) # provide the threshold at initialisation
path = graph.a_star((7, 4), (14, 18))
print(path)