import tornado.web
import numpy
from pylab import *
import copy
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
map_grid = numpy.full((100, 100, 100), int(10), dtype=numpy.int8)
map_grid[2:12, 3:8, 5:9] = 0
map_grid[5:8, 4:10, 13:16] = 0
map_grid[3:10, 7:9, 6:8] = 0
map_grid[10:14, 3:8, 8:10] = 0
map_grid[17:19, 13:17, 3:14] = 0
map_grid[10:17, 3:10, 13:19] = 0
map_grid[10:15, 8:16, 13:17] = 0
map_grid[2, 2, 2] = 7
map_grid[99, 99, 99] = 5
def min_index(data):
index = []
dim_1 = data.ravel()
min_n = min(dim_1)
for i in range(len(dim_1)):
if dim_1[i] == min_n:
pos = np.unravel_index(i, data.shape, order='C')
index.append(pos)
return np.array(index)
def select_index(data, value):
index = []
dim_1 = data.ravel()
for i in range(len(dim_1)):
if dim_1[i] == value:
pos = np.unravel_index(i, data.shape, order='C')
index.append(pos)
return np.array(index)
class AStar(object):
def __init__(self):
self.start = numpy.array([2, 2, 2])
self.goal = numpy.array([99, 99, 99])
self.open = numpy.array([[], [], [], [], [], [], [], []])
self.closed = numpy.array([[], [], [], [], [], [], [], []])
self.best_path_array = numpy.array([[], [], []])
def h_value_tem(self, son_p):
"""
计算拓展节点和终点的h值
:param son_p: 子搜索节点坐标
:return:
"""
h = (son_p[0] - self.goal[0]) ** 2 + (son_p[1] - self.goal[1]) ** 2 + (son_p[2] - self.goal[2]) ** 2
h = numpy.sqrt(h)
return h
def g_accumulation(self, son_point, father_point):
"""
累计的g值
:return:
"""
g1 = father_point[0] - son_point[0]
g2 = father_point[1] - son_point[1]
g3 = father_point[2] - son_point[2]
g = g1 ** 2 + g2 ** 2 + g3 ** 2
g = numpy.sqrt(g) + father_point[-2]
return g
def f_value_tem(self, son_p, father_p):
"""
求出的是临时g值和h值加上累计g值得到全局f值
:param father_p: 父节点坐标
:param son_p: 子节点坐标
:return:f
"""
f = self.g_accumulation(son_p, father_p) + self.h_value_tem(son_p)
return f
def child_point(self, x):
"""
拓展的子节点坐标
:param x: 父节点坐标
:return: 子节点存入open表,返回值是每一次拓展出的子节点数目,用于撞墙判断
当搜索的节点撞墙后,如果不加处理,会陷入死循环
"""
for j in range(-1, 2, 1):
for q in range(-1, 2, 1):
for p in range(-1, 2, 1):
if j == 0 and q == 0 and p ==0:
continue
m = [x[0] + j, x[1] + q, x[2] + p]
if m[0] < 0 or m[0] > 99 or m[1] < 0 or m[1] > 99 or m[2] < 0 or m[2] > 99:
continue
if map_grid[int(m[0]), int(m[1]), int(m[2])] == 0:
continue
record_g = self.g_accumulation(m, x)
record_f = self.f_value_tem(m, x)
x_direction, y_direction, z_direction = self.direction(x, m)
para = [m[0], m[1], m[2], x_direction, y_direction, z_direction, record_g, record_f]
a, index = self.judge_location(m, self.open)
if a == 1:
if record_f <= self.open[7][index]:
self.open[7][index] = record_f
self.open[6][index] = record_g
self.open[5][index] = z_direction
self.open[4][index] = y_direction
self.open[3][index] = x_direction
continue
b, index2 = self.judge_location(m, self.closed)
if b == 1:
if record_f <= self.closed[7][index2]:
self.open[7][index2] = record_f
self.open[6][index2] = record_g
self.open[5][index2] = z_direction
self.open[4][index2] = y_direction
self.open[3][index2] = x_direction
self.closed = numpy.delete(self.closed, index2, axis=1)
self.open = numpy.c_[self.open, para]
continue
self.open = numpy.c_[self.open, para]
def judge_location(self, m, list_co):
"""
判断拓展点是否在open表或者closed表中
:return:返回判断是否存在,和如果存在,那么存在的位置索引
"""
jud = 0
index = 0
for i in range(list_co.shape[1]):
if m[0] == list_co[0, i] and m[1] == list_co[1, i] and m[2] == list_co[2, i]:
jud = jud + 1
index = i
break
else:
jud = jud
return jud, index
def direction(self, father_point, son_point):
"""
建立每一个节点的方向,便于在closed表中选出最佳路径
非常重要的一步,不然画出的图像参考1.1版本
x记录子节点和父节点的x轴变化
y记录子节点和父节点的y轴变化
如(0,1)表示子节点在父节点的方向上变化0和1
:return:
"""
x = son_point[0] - father_point[0]
y = son_point[1] - father_point[1]
z = son_point[2] - father_point[2]
return x, y, z
def path_backtrace(self):
"""
回溯closed表中的最短路径
:return:
"""
best_path = list(self.goal)
self.best_path_array = numpy.array([[99], [99], [99]])
j = 0
close = self.closed
print(close)
while j <= self.closed.shape[1]:
for i in range(self.closed.shape[1]):
if best_path[0] == self.closed[0][i] and best_path[1] == self.closed[1][i] and best_path[2] == self.closed[2][i]:
x = self.closed[0][i] - self.closed[3][i]
y = self.closed[1][i] - self.closed[4][i]
z = self.closed[2][i] - self.closed[5][i]
best_path = [x, y, z]
self.best_path_array = numpy.c_[self.best_path_array, best_path]
break
else:
continue
j = j + 1
return self.best_path_array
def main(self):
"""
main函数
:return:
"""
best = self.start
h0 = self.h_value_tem(best)
init_open = [best[0], best[1], best[2], 0, 0, 0, 0, h0]
self.open = numpy.column_stack((self.open, init_open))
ite = 1
while ite <= 1000000:
if self.open.shape[1] == 0:
print('没有搜索到路径!')
return
self.open = self.open.T[numpy.lexsort(self.open)].T
best = self.open[:, 0]
print('检验第%s次当前点坐标*******************' % ite)
print(best)
self.closed = numpy.c_[self.closed, best]
if best[0] == 99 and best[1] == 99 and best[2] == 99:
print('搜索成功!')
return
self.child_point(best)
self.open = numpy.delete(self.open, 0, axis=1)
ite = ite + 1
def draw_map(a):
fig = plt.figure()
ax = Axes3D(fig)
ob_index = min_index(map_grid)
ob_x = ob_index[:,0]
ob_y = ob_index[:,1]
ob_z = ob_index[:,2]
ax.scatter(ob_x, ob_y, ob_z, s=30, c='b')
ax.scatter(2, 2, 2, s=30, c='r')
ax.scatter(99, 99, 99, s=30, c='r')
print('打印direction长度:')
print(a.best_path_array.shape[1])
best_index = a.best_path_array
best_x = best_index[0,:]
best_y = best_index[1,:]
best_z = best_index[2,:]
close = a.closed
print(best_x)
ax.scatter(best_x, best_y, best_z, s=30, c='r')
print(close)
plt.plot(best_x, best_y, best_z, lw=5, c='r')
ax.set_zlabel('Z', fontdict={
'size': 15, 'color': 'red'})
ax.set_ylabel('Y', fontdict={
'size': 15, 'color': 'red'})
ax.set_xlabel('X', fontdict={
'size': 15, 'color': 'red'})
plt.show()
if __name__ == '__main__':
a1 = AStar()
a1.main()
a1.path_backtrace()
draw_map(a1)