Алгоритм A-star приводит к странным, медленным и неэффективным выборам

Question

Сегодня я попытался создать алгоритм поиска пути A-STAR с визуализацией в PyGame. Код:

import pygame, time

pygame.init()
pygame.font.init()
#----------[0,1,2]
#GRID[x][y][g/h/f]
#g_cost: distance from starting to current node
#h_cost: distance from ending to current node
#f_cost: sum of previous two costs. That way, the lower
#the f cost, the more likely a node is to be on the path

WIDTH = 20
HEIGHT = 15
XSTART = 5
YSTART = 5
XEND = 15
YEND = 10
TICK = 10000000
CLOCK = pygame.time.Clock()
OPEN_NODES = []
CLOSED_NODES = []
BEACON = 0
SQUARE = 50
GEN = 0
WHITE = (255,255,255)

GRID = []

#func to find distance between 2 nodes
def find_dist(coor1,coor2):
    x1,y1 = coor1
    x2,y2 = coor2
    x_diff = abs(x1-x2)
    y_diff = abs(y1-y2)
    smaller = min([x_diff,y_diff])
    #diagonal multiplied by sqrt(2) approximately
    dist = round(smaller*1.4+(max([x_diff,y_diff])-smaller))*10
    return dist

#super stupid way to get all node neighbors
def get_neighbors(coors):
    x,y = coors
    neighbors = []
    if x-1 > -1:
        neighbors.append(GRID[x-1][y])
    if x-1 > -1 and y+1 < HEIGHT:   
        neighbors.append(GRID[x-1][y+1])
    if y+1 < HEIGHT:
        neighbors.append(GRID[x][y+1])
    if y+1 < HEIGHT and x+1 < WIDTH:
        neighbors.append(GRID[x+1][y+1])
    if x+1 < WIDTH:
        neighbors.append(GRID[x+1][y])
    if x+1 < WIDTH and y-1 > -1:
        neighbors.append(GRID[x+1][y-1])
    if y-1 > -1:
        neighbors.append(GRID[x][y-1])
    if y-1 > -1 and x-1 > -1:
        neighbors.append(GRID[x-1][y-1])
    return neighbors

#returns list without duplicates
def elim_duplicates(listin):
    for element in listin:
        if listin.count(element) > 1:
            listin.pop(listin.index(element))
    return listin


#creates main grid
for x in range(WIDTH):
    COLUMN = []
    for y in range(HEIGHT):
        COLUMN.append([-1,-1])
    GRID.append(COLUMN)
GRID[XSTART][YSTART] = [-2,-2]
GRID[XEND][YEND] = [-3,-3]

#adds costs to each node
for x in range(WIDTH):
    for y in range(HEIGHT):
        g_cost = find_dist([x,y],[XSTART,YSTART])
        h_cost = find_dist([x,y],[XEND,YEND])
        f_cost = g_cost + h_cost
        GRID[x][y] = [g_cost,h_cost,f_cost,x,y]

#adds the starting node to open nodes,
#which is a list containing all nodes
#that will be checked for the lowest f cost
OPEN_NODES.append(GRID[XSTART][YSTART])

WINDOW = pygame.display.set_mode((WIDTH*SQUARE, HEIGHT*SQUARE))
pygame.display.set_caption('A* Algorithm')

RUN = True

PATH_NODES = []

while RUN:
    GEN += 1
    CLOCK.tick(TICK)
    #cycle control
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            RUN = False
    #updates costs
    for x in range(WIDTH):
        for y in range(HEIGHT):
            g_cost = find_dist([x,y],[XSTART,YSTART])
            h_cost = find_dist([x,y],[XEND,YEND])
            f_cost = g_cost + h_cost
            GRID[x][y] = [g_cost,h_cost,f_cost,x,y]
    #creates a list with only f costs
    f_cost_list = []
    for i in OPEN_NODES:
        f_cost_list.append(i[2])
    #finds the node with the lowest f cost
    CURRENT_NODE = OPEN_NODES[f_cost_list.index(min(f_cost_list))]
    #adds all neighbors of the current node and removes duplicates
    OPEN_NODES.extend(get_neighbors(CURRENT_NODE[3:]))
    OPEN_NODES = elim_duplicates(OPEN_NODES)
    #adds current node to possible path nodes
    PATH_NODES.append(CURRENT_NODE)
    #removes current node from open nodes
    OPEN_NODES.remove(CURRENT_NODE)
    #removes all possible duplicates from lists
    PATH_NODES = elim_duplicates(PATH_NODES)
    OPEN_NODES = elim_duplicates(OPEN_NODES)
    #checks if path has been found
    if CURRENT_NODE[3] == XEND and CURRENT_NODE[4] == YEND:
        pygame.draw.rect(WINDOW,(0,255,255), (490, 490, SQUARE, SQUARE))
        print('DONE')
        time.sleep(3)
        pygame.quit()
    #draws all open nodes
    for i in OPEN_NODES:
        pygame.draw.rect(WINDOW,(0,0,255),(i[3]*SQUARE,i[4]*SQUARE,SQUARE,SQUARE))
    #draws all possible path nodes
    for i in PATH_NODES:
        pygame.draw.rect(WINDOW,(255,0,0),(i[3]*SQUARE,i[4]*SQUARE,SQUARE,SQUARE))
    #draws start and end nodes
    pygame.draw.rect(WINDOW,(0,255,0), (XSTART*SQUARE, YSTART*SQUARE, SQUARE, SQUARE))
    pygame.draw.rect(WINDOW,(0,255,0), (XEND*SQUARE,YEND*SQUARE,SQUARE,SQUARE))
    #shows generation
    font = pygame.font.Font(None, 100)
    text = font.render(str(GEN), 1, WHITE)
    WINDOW.blit(text, (10,10))
    #updates screen
    pygame.display.update()
    WINDOW.fill((0, 0, 0))
pygame.quit()

Всякий раз, когда я запускаю алгоритм с начальной и конечной координатами, как показано, это странный результат, который показывает:

Он еще не показывает точный путь, но ошибки в алгоритме хорошо видны: дыры, странные ребра и т. Д. c.

Что может быть причиной этого?