68 lines
2.7 KiB
Python
68 lines
2.7 KiB
Python
from config import *
|
|
import heapq
|
|
|
|
class Astar():
|
|
|
|
def __init__(self,game):
|
|
|
|
self.g = game
|
|
|
|
# Define the movement directions (up, down, left, right)
|
|
self.directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
|
|
|
|
def heuristic(self,a, b):
|
|
# Calculate the Manhattan distance between two points
|
|
return abs(b[0] - a[0]) + abs(b[1] - a[1])
|
|
|
|
def get_successors(self,position):
|
|
# Get the neighboring cells that can be traversed
|
|
successors = []
|
|
for direction in self.directions:
|
|
neighbor = (position[0] + direction[0], position[1] + direction[1])
|
|
if 0 <= neighbor[0] < TILE_SIZE and 0 <= neighbor[1] < TILE_SIZE and self.g.obstacles[neighbor[0]][neighbor[1]] == False:
|
|
successors.append(neighbor)
|
|
return successors
|
|
|
|
def print_path(self,came_from, current,path):
|
|
# Recursively print the path from the start to the current position
|
|
if current in came_from:
|
|
path = self.print_path(came_from, came_from[current],path)
|
|
path.append(self.g.bfs.get_cell_number(current[0]*TILE_SIZE,current[1]*TILE_SIZE))
|
|
#print("Budowanie ścieżki: ",path)
|
|
return path
|
|
|
|
def a_star(self, goal):
|
|
path = []
|
|
start = (self.g.agent.rect.x//TILE_SIZE, self.g.agent.rect.y//TILE_SIZE)
|
|
#print(start,goal)
|
|
open_set = []
|
|
heapq.heappush(open_set, (0, start)) # Priority queue with the start position
|
|
came_from = {}
|
|
g_scores = {start: 0} # Cost from start to each position
|
|
f_scores = {start: self.heuristic(start, goal)} # Total estimated cost from start to goal through each position
|
|
|
|
while open_set:
|
|
_, current = heapq.heappop(open_set)
|
|
|
|
if current == goal:
|
|
# Goal reached, print the path
|
|
path = self.print_path(came_from, goal,path)
|
|
return path
|
|
|
|
for successor in self.get_successors(current):
|
|
# Calculate the cost to move from the current position to the successor
|
|
cost = self.g.cell_costs[successor[0]][successor[1]]
|
|
tentative_g_score = g_scores[current] + cost
|
|
|
|
if successor not in g_scores or tentative_g_score < g_scores[successor]:
|
|
# Update the cost and priority if it's a better path
|
|
came_from[successor] = current
|
|
g_scores[successor] = tentative_g_score
|
|
f_scores[successor] = tentative_g_score + self.heuristic(successor, goal)
|
|
heapq.heappush(open_set, (f_scores[successor], successor))
|
|
# No path found
|
|
print("No path found.")
|
|
|
|
|
|
|