67 lines
2.1 KiB
Python
Executable File
67 lines
2.1 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
import problem
|
|
|
|
|
|
def astar_heuristic(start, goal):
|
|
side = 1 if start.boat_side == "left" else 0
|
|
return (start.left_cats + start.left_dogs) * (1 - side) * 5 + (
|
|
start.right_dogs) * side * 5
|
|
|
|
|
|
def astar_search(start, end):
|
|
actual_cost = {}
|
|
estimated_cost = {}
|
|
|
|
actual_cost[start] = 0
|
|
estimated_cost[start] = astar_heuristic(start, end)
|
|
|
|
seen_nodes = set()
|
|
unseen_nodes = set([start])
|
|
|
|
previous_node = None
|
|
while len(unseen_nodes) > 0:
|
|
current = None
|
|
current_action = None
|
|
current_estimate = None
|
|
for item in unseen_nodes:
|
|
if current is None or estimated_cost[item] < current_estimate:
|
|
current_estimate = estimated_cost[item]
|
|
current = item
|
|
if previous_node is not None:
|
|
current_action = previous_node.get_action_to(current)
|
|
if current_action is None:
|
|
current_action = current.get_action_from(seen_nodes)
|
|
unseen_nodes.remove(current)
|
|
seen_nodes.add(current)
|
|
if current == problem.start:
|
|
print("Initial state")
|
|
print(current.to_string())
|
|
else:
|
|
if current_action is not None:
|
|
print(current_action.to_string(),
|
|
"(cost: " + str(current_estimate) + ")")
|
|
print(current.to_string())
|
|
else:
|
|
print(current.to_string())
|
|
if current == end:
|
|
print("Goal state found")
|
|
return current
|
|
previous_node = current
|
|
for act in current.actions:
|
|
child = act.result_state
|
|
if child in seen_nodes:
|
|
continue
|
|
child_actual = actual_cost[current] + 1
|
|
if child not in unseen_nodes:
|
|
unseen_nodes.add(child)
|
|
elif child_actual >= actual_cost[child]:
|
|
continue
|
|
actual_cost[child] = child_actual
|
|
estimated_cost[child] = actual_cost[child] + astar_heuristic(
|
|
child, end)
|
|
print("Goal state could not be found")
|
|
|
|
|
|
finish = astar_search(problem.start, problem.end)
|