def test_case2(self):
start = (0, 0)
goal = (3, 3)
came_from, cost_so_far = a_star_search(self.g2, start, goal)
path = reconstruct_path(came_from, start, goal)
answer = [(0, 0), (1, 1), (2, 2), (3, 3)]
self.assertEqual(path, answer)
def multiple_a_star_search(graph, segment, heuristic=diagonal_distance, p=0):
path = []
for i in range(1, len(segment)):
start = segment[i - 1]
goal = segment[i]
if type(graph) in SUPPORT_SET:
graph.set_search(start, goal)
came_from, cost_so_far = a_star_search(graph, start, goal, heuristic,
p)
temp_path = reconstruct_path(came_from, start, goal)
if i != len(segment) - 1:
path += temp_path[:-1]
else:
path += temp_path
return path
def preload(self, start, end):
self.walls.clear()
start_sec = self.get_s2_section(start)
end_sec = self.get_s2_section(end)
self.walls |= self.walls_s2 - set([start_sec, end_sec])
self.set_search(start_sec, end_sec)
came_from, cost_so_far = a_star_search(self, start_sec, end_sec)
path_sec = reconstruct_path(came_from, start_sec, end_sec)
blocks = []
for sec in path_sec:
if self.in_sections(sec):
blocks += self.get_candidates(sec)
blocks = set(blocks) & self.walls_s2
visited = {}
expand = set()
for block in blocks:
expand |= self.bfs_blocks(block, visited)
self.walls.clear()
for sec in expand:
self.load_section(sec)
self.walls = self.walls_s1
# came_from, cost_so_far = a_star_search(grid1, start3, goal3, p=p)
# path = reconstruct_path(came_from, start3, goal3)
# path3 = reduce_path(path)
# for i in range(1, len(path3)):
# grid1.walls |= solid_octagon_line(path3[i-1], path3[i], 5)
#
# print("Grid 1 Time Elapsed: {:.4f} sec.".format(time.time() - t0))
t0 = time.time()
print('4')
grid2.set_search(start1, goal1)
t1 = time.time()
came_from, cost_so_far = a_star_search(grid2, start1, goal1, p=p)
ts1 = time.time() - t1
path = reconstruct_path(came_from, start1, goal1)
path1_ = reduce_path(path)
for i in range(1, len(path1_)):
grid2.walls |= solid_octagon_line(path1_[i - 1], path1_[i], 5)
print('5')
grid2.set_search(start2, goal2)
t2 = time.time()
came_from, cost_so_far = a_star_search(grid2, start2, goal2, p=p)
ts2 = time.time() - t2
path = reconstruct_path(came_from, start2, goal2)
path2_ = reduce_path(path)
for i in range(1, len(path2_)):
grid2.walls |= solid_octagon_line(path2_[i - 1], path2_[i], 5)
print('6')
grid2.set_search(start3, goal3)
t3 = time.time()
if __name__ == '__main__':
from shape.OctagonLine import solid_octagon_line
import matplotlib.pyplot as plt
graph = TwoStageGraph(10000, 10000, 100)
print('*')
blocks = solid_octagon_line((550, 500), (550, 5500), 20)
print(len(blocks))
# for pos in solid_octagon_line((550, 500), (550, 5500), 20):
# graph.add_wall(pos)
graph.add_walls(solid_octagon_line((550, 500), (550, 5500), 20))
print('**')
start, end = (500, 500), (600, 5100)
graph.preload(start, end)
graph.set_search(start, end)
print(len(graph.walls))
came_from, cost_so_far = a_star_search(graph, start, end)
path = reconstruct_path(came_from, start, end)
plt.figure()
plt.scatter([pos[0] for pos in blocks], [pos[1] for pos in blocks],
color='black')
plt.scatter([pos[0] for pos in graph.walls],
[pos[1] for pos in graph.walls],
color='orange')
for i in range(1, len(path)):
plt.plot([path[i - 1][0], path[i][0]], [path[i - 1][1], path[i][1]],
color='red')
plt.scatter([pos[0] for pos in grid.sights],
[pos[1] for pos in grid.sights],
color='orange')
plt.scatter([pos[0] for pos in grid.outlines],
[pos[1] for pos in grid.outlines],
color='yellow')
plt.scatter([pos[0] for pos in grid.search],
[pos[1] for pos in grid.search],
color='green')
# vertex
plt.scatter([pos[0] for pos in grid.vertex.keys()],
[pos[1] for pos in grid.vertex.keys()],
color='blue')
# edge
for key in grid.edge.keys():
pt1, pt2 = key
plt.plot([pt1[0], pt2[0]], [pt1[1], pt2[1]], color='green')
came_from, cost_so_far = a_star_search(grid, start, goal, p=p)
path = reconstruct_path(came_from, start, goal)
path = reduce_path(path)
for i in range(1, len(path)):
grid.walls |= solid_octagon_line(path[i - 1], path[i], 5)
for i in range(1, len(path)):
plt.plot([path[i - 1][0], path[i][0]], [path[i - 1][1], path[i][1]],
color='red')