我正在研究A-star算法的Python代码，并且想在3D environment中实现它。我该如何添加第三个尺寸，直到将其绘制为3D尺寸，具体我需要做些什么？如何更改或修改地图，直到将其绘制为3D？我可以得到任何指导或帮助吗？。
这是在我的代码下面：
import random
import math
grid = [[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0]]
heuristic = [[9, 8, 7, 6, 5, 4],
[8, 7, 6, 5, 4, 3],
[7, 6, 5, 4, 3, 2],
[6, 5, 4, 3, 2, 1],
[5, 4, 3, 2, 1, 0]]
init = [1,0] #Start location is (1,0) which we put it in open list.
goal = [len(grid)-1,len(grid[0])-1] #Our goal in (4,5) and here are the coordinates of the cell.
#Below the four potential actions to the single field
delta = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
delta_name = ['V','>','<','^','//','\\','\\','//']
cost = 1 #Each step costs you one
def search():
closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
'''
Here we are making field as the same size as the grid, we memorize for each cell what action it took to get there.
'''
action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
#We initialize the starting location as checked
closed[init[0]][init[1]] = 1
expand=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
# we assigned the cordinates and g value
x = init[0]
y = init[1]
g = 0
h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
#h = heuristic[x][y]
f = g + h
#our open list will contain our initial value
open = [[f, g, h, x, y]]
found = False #flag that is set when search complete
resign = False #Flag set if we can't find expand
count = 0
#print('initial open list:')
#for i in range(len(open)):
#print(' ', open[i])
#print('----')
while found is False and resign is False:
#Check if we still have elements in the open list
if len(open) == 0: #If our open list is empty, there is nothing to expand.
resign = True
print('Fail')
print('############# Search terminated without success')
print()
else:
#if there is still elements on our list
#remove node from list
open.sort() #sort elements in an increasing order from the smallest g value up
open.reverse() #reverse the list
next = open.pop() #remove the element with the smallest g value from the list
#print('list item')
#print('next')
#Then we assign the three values to x,y and g. Which is our expantion.
x = next[3]
y = next[4]
g = next[1]
expand[x][y] = count
count+=1
#Check if we are done
if x == goal[0] and y == goal[1]:
found = True
print(next) #The three elements above this "if".
print('############## Search is success')
print()
else:
#expand winning element and add to new open list
for i in range(len(delta)): #going through all our actions the four actions
x2 = x + delta[i][0]
y2 = y + delta[i][1]
#if x2 and y2 falls into the grid
if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <= len(grid[0])-1:
#if x2 and y2 not checked yet and there is not obstacles
if closed[x2][y2] == 0 and grid[x2][y2] == 0:
g2 = g + cost #we increment the cose
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
#h2 = heuristic[x2][y2]
f2 = g2 + h2
open.append([f2,g2,h2,x2,y2]) #we add them to our open list
#print('append list item')
#print([g2,x2,y2])
#Then we check them to never expand again
closed[x2][y2] = 1
action[x2][y2] = i
for i in range(len(expand)):
print(expand[i])
print()
policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
x=goal[0]
y=goal[1]
policy[x][y]='*'
while x !=init[0] or y !=init[1]:
x2=x-delta[action[x][y]][0]
y2=y-delta[action[x][y]][1]
policy[x2][y2]= delta_name[action[x][y]]
x=x2
y=y2
for i in range(len(policy)):
print(policy[i])
search()