mirror of
https://github.com/Andreabont/PyExperiments.git
synced 2024-09-19 14:43:05 +00:00
103 lines
2.6 KiB
Python
103 lines
2.6 KiB
Python
|
# -*- coding: utf-8 -*-
|
||
|
from Queue import *
|
||
|
|
||
|
def breadth_first_search(graph, start, goal):
|
||
|
frontier = Queue()
|
||
|
frontier.put(start)
|
||
|
came_from = {}
|
||
|
came_from[start] = None
|
||
|
|
||
|
while not frontier.empty():
|
||
|
current = frontier.get()
|
||
|
|
||
|
if current == goal:
|
||
|
break
|
||
|
|
||
|
for next in graph.neighbors(current):
|
||
|
if next not in came_from:
|
||
|
frontier.put(next)
|
||
|
came_from[next] = current
|
||
|
|
||
|
return came_from
|
||
|
|
||
|
def dijkstra_search(graph, start, goal):
|
||
|
frontier = PriorityQueue()
|
||
|
frontier.put(start, 0)
|
||
|
came_from = {}
|
||
|
cost_so_far = {}
|
||
|
came_from[start] = None
|
||
|
cost_so_far[start] = 0
|
||
|
|
||
|
while not frontier.empty():
|
||
|
current = frontier.get()
|
||
|
|
||
|
if current == goal:
|
||
|
break
|
||
|
|
||
|
for next in graph.neighbors(current):
|
||
|
new_cost = cost_so_far[current] + graph.cost(current, next)
|
||
|
if next not in cost_so_far or new_cost < cost_so_far[next]:
|
||
|
cost_so_far[next] = new_cost
|
||
|
priority = new_cost
|
||
|
frontier.put(next, priority)
|
||
|
came_from[next] = current
|
||
|
|
||
|
return came_from
|
||
|
|
||
|
def a_star_search(graph, start, goal):
|
||
|
frontier = PriorityQueue()
|
||
|
frontier.put(start, 0)
|
||
|
came_from = {}
|
||
|
cost_so_far = {}
|
||
|
came_from[start] = None
|
||
|
cost_so_far[start] = 0
|
||
|
|
||
|
while not frontier.empty():
|
||
|
current = frontier.get()
|
||
|
|
||
|
if current == goal:
|
||
|
break
|
||
|
|
||
|
for next in graph.neighbors(current):
|
||
|
new_cost = cost_so_far[current] + graph.cost(current, next)
|
||
|
if next not in cost_so_far or new_cost < cost_so_far[next]:
|
||
|
cost_so_far[next] = new_cost
|
||
|
priority = new_cost + heuristic(goal, next)
|
||
|
frontier.put(next, priority)
|
||
|
came_from[next] = current
|
||
|
|
||
|
return came_from
|
||
|
|
||
|
def heuristic(a, b):
|
||
|
(x1, y1) = a
|
||
|
(x2, y2) = b
|
||
|
return abs(x1 - x2) + abs(y1 - y2)
|
||
|
|
||
|
def reconstruct_path(came_from, start, goal):
|
||
|
current = goal
|
||
|
path = [current]
|
||
|
while current != start:
|
||
|
current = came_from[current]
|
||
|
path.append(current)
|
||
|
path.reverse()
|
||
|
return path
|
||
|
|
||
|
def arrow_draw(global_path, current):
|
||
|
if current not in global_path.keys():
|
||
|
return '.'
|
||
|
if global_path[current] is None:
|
||
|
return '.'
|
||
|
(cx, cy) = current
|
||
|
(nx, ny) = global_path[current]
|
||
|
if cx-nx > 0:
|
||
|
return '→'
|
||
|
elif cx-nx < 0:
|
||
|
return '←'
|
||
|
elif cy-ny > 0:
|
||
|
return '↑'
|
||
|
elif cy-ny < 0:
|
||
|
return '↓'
|
||
|
else:
|
||
|
return '.'
|
||
|
|