def try_to_connect(new_node, base_node, nodes, edges, obstacles):
if new_node in nodes or new_node in obstacles:
return False, nodes, edges
new_edge = (base_node, new_node)
for obstacle in obstacles:
for edge in get_edges_around(*obstacle):
if intersected(edge, new_edge):
return False, nodes, edges
try:
gm.set_cell(*new_node, gm.CELL_NODE)
gm.ax.plot([new_node[0], base_node[0]], [new_node[1], base_node[1]],
linewidth=WIDTH_EDGE_ALGORITHM,
color=COLOR_EDGE_ALGORITHM)
except RuntimeError:
return False, nodes, edges
plot_node_bell.play()
nodes.append(new_node)
edges.append(new_edge)
return True, nodes, edges
def switch_mode(new_mode, title=None):
MODE_TITLE = {MODE_RUNNING: 'RUNNING', MODE_DRAWING: 'DRAWING'}
if new_mode not in (MODE_RUNNING, MODE_DRAWING):
raise ValueError('unexpected mode given')
global mode
mode = new_mode
if title is None:
gm.set_title(MODE_TITLE[mode])
else:
gm.set_title(title)
if mode == MODE_RUNNING:
global thread_algorithm
thread_algorithm = Thread(target=algorithm)
thread_algorithm.start()
if mode == MODE_DRAWING:
gm.set_cell(*NODE_START, gm.CELL_START)
gm.set_cell(*NODE_GOAL, gm.CELL_GOAL)
def set_unset_obstacles_handler(event):
global mode
if mode != MODE_DRAWING:
return
x, y = event.xdata, event.ydata
if x is None or y is None:
return
if event.button == gm.BUTTON_LEFTCLICK:
gm.set_cell(x, y, gm.CELL_OBSTACLE)
gm.set_title('DRAWING')
elif event.button == gm.BUTTON_RIGHTCLICK:
gm.set_cell(x, y, gm.CELL_EMPTY)
gm.set_title('DRAWING')
gm.set_cell(*NODE_START, gm.CELL_START)
gm.set_cell(*NODE_GOAL, gm.CELL_GOAL)
def clear_all_nodes():
objects_like_node = set(
(gm.CELL_START, gm.CELL_GOAL, gm.CELL_SHORTESTNODE, gm.CELL_NODE))
for y in range(gm.height):
for x in range(gm.width):
if gm.get_cell(x, y) in objects_like_node:
gm.set_cell(x, y, gm.CELL_EMPTY)
gm.set_cell(*NODE_START, gm.CELL_START)
gm.set_cell(*NODE_GOAL, gm.CELL_GOAL)
def clear_all_cells():
for y in range(gm.height):
for x in range(gm.width):
gm.set_cell(x, y, gm.CELL_EMPTY)
def algorithm():
clear_all_nodes()
clear_all_edges()
try:
gm.set_cell(*NODE_START, gm.CELL_START)
gm.set_cell(*NODE_GOAL, gm.CELL_GOAL)
except RuntimeError:
return
time.sleep(INTERVAL_ALGORITHM)
nodes = [NODE_START]
edges = []
try:
obstacles = [(x, y) for x in range(gm.width) for y in range(gm.height)
if gm.get_cell(x, y) == gm.CELL_OBSTACLE]
except RuntimeError:
return
global mode
retry_count = 0
while mode == MODE_RUNNING and retry_count < TIMEOUT_ALGORITHM:
if random.random() <= GOAL_RATIO_ALGORITHM:
target_to_extend = NODE_GOAL
else:
try:
target_to_extend = (random.randint(0, gm.width - 1),
random.randint(0, gm.height - 1))
except RuntimeError:
return
if target_to_extend in nodes:
retry_count += 1
continue
# targetの最近傍ノード
node_nearest = sorted(
nodes, key=lambda n: distance_of_nodes(n, target_to_extend))[0]
# 最近傍ノードからターゲットへSTEP_LENGTH_ALGORITHM分移動したノード（候補ノード）
distance = distance_of_nodes(target_to_extend, node_nearest)
if distance < STEP_LENGTH_ALGORITHM:
scale = 1
else:
scale = STEP_LENGTH_ALGORITHM / distance
x_candidate = node_nearest[0] + scale * (target_to_extend[0] -
node_nearest[0])
y_candidate = node_nearest[1] + scale * (target_to_extend[1] -
node_nearest[1])
try:
node_candidate = gm.get_index(x_candidate, y_candidate)
except RuntimeError:
return
ok, nodes, edges = try_to_connect(node_candidate, node_nearest, nodes,
edges, obstacles)
if not ok:
retry_count += 1
continue
if node_candidate == NODE_GOAL:
nodes_of_path = find_nodes_of_path_with_dijkstra(nodes, edges)
for node in nodes_of_path:
try:
gm.set_cell(*node, gm.CELL_SHORTESTNODE)
except RuntimeError:
return
try:
gm.set_cell(*NODE_START, gm.CELL_START)
gm.set_cell(*NODE_GOAL, gm.CELL_GOAL)
x_series = [node[0] for node in nodes_of_path]
y_series = [node[1] for node in nodes_of_path]
gm.ax.plot(x_series,
y_series,
linewidth=WIDTH_SHORTESTEDGE_ALGORITHM,
color=COLOR_SHORTESTEDGE_ALGORITHM)
except RuntimeError:
return
search_succeeded_bell.play()
switch_mode(MODE_DRAWING,
'SUCCESS ({} steps)'.format(len(nodes_of_path) - 1))
return
retry_count = 0
time.sleep(INTERVAL_ALGORITHM)
switch_mode(MODE_DRAWING, 'FAILED or ABORTED')